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Preface 



Over the last few decades, both aeronautics and astronautics have been strong mo- 
tors for the advancement of control systems theory and application, as well as the 
fields of sensors, data fusion and navigation. Many of those achievements that 
earned aerospace the reputation as a synonym for high tech and progress have 
been enabled by innovations in guidance, navigation and control. 

Today, aerospace is still one of the driving applications in those fields, which 
stems from the special characteristics and needs of that segment. In an airplane 
seating hundreds of people, you cannot use the latest control algorithm to find out 
if it works by trial and error. In a deep space probe sailing for new shores, you 
need to do things right on the first and only attempt; (unlike a car or a microwave 
oven) you cannot test the integrated system in real operation before its actual mis- 
sion. You cannot simply put a bulky machined box with standard components 
which works in "living room" environmental conditions in the slender body of an 
agile missile. 

Things are also quite different on the algorithm side. Aerospace systems 
can have highly nonlinear and strongly coupled dynamics. The ranges of altitude, 
Mach number, center of gravity and weight are enormous and the dynamics can 
significantly change with those parameters. Huge uncertainties can still remain 
in spite of costly modeling efforts. The range of time scales contributing to the 
system dynamics is large, speeds are higher, the environment is harsher and 
changing more rapidly, the distances travelled are much larger and the operation 
times for some systems are much longer than in other fields. To summarize, the 
challenges in the aerospace disciplines are unique and more demanding than in 
other domains. 

If these challenges were not enough, appropriate solutions must also be reliable, 
highly accurate, highly available, safe and must guarantee a well-defined perform- 
ance level, even under a large variety of circumstances like system failure. An 
airplane cannot turn right and stop at the next cloud if things go wrong. All 
these challenges must be accomplished under mass, volume, power and cost 
constraints. 

This may sound like praise for aerospace, its scientists and engineers, but how- 
ever you wish to see it, it is a viable explanation as to why, in contrast to other 
fields, there have always been dedicated conferences on "flight control", "space 
navigation" and "missile guidance", as specialized sessions at general conferences 
are not enough. The American AIAA Guidance, Navigation and Control confer- 
ence serves as a brilliant example where the community of "rocket scientists" 
gathers to present on and to discuss these specific topics. 



VI Preface 

Europe has seen a strong trans-national consolidation process in aerospace over 
the previous decades. Most of the visible products, be it commercial aircraft, 
fighters, helicopters, satellites, launchers or missiles, are not made by a single 
country - they are the fruits of cooperation. No European country by itself hosts a 
specialized GNC community large enough to cover the whole spectrum of disci- 
plines. However, on a European scale, mutual exchange of ideas, concepts and so- 
lutions is enriching for all. Thus, the interest in having a truly European GNC con- 
ference collecting the ideas of the scientific community within the continent and 
inviting the whole world to join has been articulated quite frequently. The first 
CEAS Specialist Conference on Aerospace Guidance, Navigation and Control 
(EuroGNC) is an attempt to follow this request and turn the vision into reality. It is 
the hope of the organizers and the Technical Committee members from all CEAS 
Member states that this conference establishes itself as a high class biannual recur- 
ring event at changing locations, which brings together the researchers, scientists, 
developers and engineers who have dedicated their professional life and their pas- 
sion to the arts of guidance, navigation and control. 

We are very thankful to AIAA for acknowledging our effort as a worthy com- 
plement to their GNC conference and for co-sponsoring the event. 

Maybe - to draw a vision - an Asian aerospace GNC conference could join the 
European event, taking turns year by year. This would allow the global commu- 
nity - beyond the yearly "jour fixe" at the AIAA GNC conference - to come to- 
gether for a second time and to intensify information exchange and cooperation in 
a truly global field of science. 

At this point, it is important to thank all of those who took the burden to make a 
beginning and to organize the first EuroGNC conference. First of all DGLR, the 
German member society of CEAS which dared to take the risk of being the first 
organizer and then all the members of the organizing and technical committees 
who bravely accepted the challenge of tampering with systems and processes for 
paper review and others which are still in their prototype stages, smoothing the 
path for those to follow. Without their time, patience, dedication and willingness 
the event would never have been possible. 

But now enjoy the book which summarizes selected contributions of scientists 
to the first CEAS EuroGNC - the European Specialist Conference on Aerospace 
Guidance, Navigation and Control, giving proof to the fact that Europe and its 
friends from around the world do make a valuable contribution to the progress of 
the field. 

Munich and Bremen Florian Holzapfel 

February 201 1 Stephan Theil 
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Atmospheric Applications 



A Decoupled Approach for Trajectory 
Generation for an Unmanned Rotorcraft 



Sven Lorenz and Florian M. Adolf 



Abstract. A decoupled approach to trajectory generation based on a cubic spline 
geometry formulation is introduced. The distinct consideration of boundary condi- 
tions yields a continuously differentiable trajectory definition such that path tracking 
errors are minimized during flight. A curvature-based, dimensionless space-filling 
curve allows to determine a suitable velocity profile along the path for hover-capable 
vehicles. Tracking of the trajectory is enabled by a conversion between the spline 
parameters and the arc length of the spline. In the past years, this approach in combi- 
nation with a suitable trajectory tracking control has been successfully flight tested 
with an unmanned helicopter. 

1 Introduction 

Autonomous flight in densely populated environment like urban terrain generally 
requires excellent maneuverability. Hence, helicopter-based UAV platforms are pre- 
ferred. Especially for dynamic high speed flight, changes like moving obstacles or 
mission updates can require such complex control platforms to be equipped with an 
onboard motion planning system. 

As the survey by Goerzen HJ points out, a sole precision tracking of a precom- 
puted trajectory is not a feasible overall solution. Dynamic constraints, atmospheric 
conditions, uncertainty in the vehicle state estimates, and limited knowledge about 
the environment may leave no chance to follow a precomputed plan precisely. Ap- 
proaches for path smoothing^ exist |2| that allow to generate continuously differ- 
entiable, timely annotated paths. Moreover, given a feasible flight control system, 
a number of path following solutions exist IJl HI even for commercial, black box 
autopilots iQ. 
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However, the integrated optimization of a motion planning task can comprise 
a whole set of hard realtime computations when aspects like task sequence order- 
ing, route planning, reactive obstacle avoidance, and vehicle control are considered. 
Thus, this work proposes a decoupled approach to motion planning. Previous work 
by Andert and Adolf [^ indicates that a thorough problem decomposition is one 
key to enable realtime sensor fusion, obstacle modeling, and 3D motion planning 
during flight. This work develops this idea further by decomposing the trajectory 
generation problem in decoupled subsequent layers. Although path and trajectory 
planning may have the same meaning, there is a fundamental difference between 
them. In this context a path is defined as the interpolation of position coordinates. 
Trajectories refer to timely annotated paths, e.g. represented by a velocity profile 
over a discretized path geometry. 

Even a hover-capable vehicle cannot perform arbitrary yaw turns during fast for- 
ward flight, such that an adequate yaw attitude command must be determined. A 
common approach is to use an instantaneous tangent along a path. A continuously 
differentiable path geometry should be preferred to enable smooth transitions. The 
trajectory will be vehicle specific. The configuration spacq^ may be altered if the 
vehicle properties or the characteristics of the environment change. 

Generally, path search uses only simplified dynamic constraints and concentrates 
on collision-free path segments, often with various geometries (e.g. linear, circle, 
spline). Then a time dimension is added to the path yielding a trajectory by defining 
the velocity profile over the path geometry. Instantaneous vehicle state estimates are 
used together with the path slope in order to provide a feasible input to a trajectory 
following controller. The nonlinear plant will be controlled by a baseline control 
system that maintains desired velocities. More details on the control system can be 
found in Q [51 . 

The following section starts with the path definition. Based on the geometry of 
the path the trajectory definition will be presented in section[3] The trajectory def- 
inition includes the determination of the velocity profile as well as an approach to 
account for acceleration limitations. Section|4]presents the trajectory following con- 
trol system. A summary of the overall approach and future research directions are 
discussed in section [5] 



2 Path Definition 

In general, a smooth path should be continuously differentiable. Due to the resulting 
complexity of generating a collision-free path through narrow passages a function 
of reduced order is selected. Most path planners avoid obstacles by generating co- 
ordinates that divide the path from start to end position into consecutive segments. 
These path segments use cubic splines that are continuously differentiable up to 
the third derivative. A large safety margin accounting for path following errors can 
therefore be avoided. Furthermore, the path planner utilized in this work assures a 



^ Algorithmic search space of possible configurations that a physical system may attain. 
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collision-free sphere volume (e.g. safety radius) of a selectable size. Note that it may 
insert other path geometries into spline segments (e. g. through narrow corridors). 

A univariate, polynomial spline is defined as a piecewise polynomial function. 
In its most general form a polynomial spline S: [a,b\ -^ 9^ consists of polynomial 
pieces /",- : [t,, T,+i] -^ 9J, where a strictly increasing sequence of real numbers is 
used between the boundaries a and b: 



To < Ti < • • • < Ta._2 < Ti-i = b. (1) 



That is, 



5(t) =Po(t),To<t<Ti, (2) 

5(t) = A(t) ,Ti <T<T2, (3) 

5(t) = A_2(t) , Xk-2 < T < Ti_i . (4) 

The given k points T,- are called knots. The vector T = [tq, . . . , T^-i] is called a knot 
vector for the spline. The knots are not equidistantly distributed in the interval \a,b], 
the spline is therefore called to be non-uniform. 

If the above described interpolation method is applied to a large number of way- 
points, comparatively high-order splines have to be selected for a feasible interpola- 
tion. Consequently, oscillations between the support points may occur. Therefore, a 
cubic spline is applied piecewise for each segment. This way, a transition condition 
at each segment boundary ensures smoothness up to the second derivative. A third- 
order spline is selected for each segment k and each degree of freedom ; = [x, y, z] : 

Si.k{T:) = ai.k + bi.k ■ (t: - ^k) + c,',i ■ (t - Tkf + dj.k ■ (t - T^,)^ (5) 

5',jt(T) represents the spline for the dimension / in segment k for a specific T. Its 
derivatives w. r. t. T are given by: 

S',.k{^) - bi^k + 2 • ci,k • (t - Ti) + 3 • d^ ■ (t - Tk? (6) 

sI^{t) ^2-Ci,k + 6-di^k-{T~Tk) (7) 

S'",{r)^6-d,k. (8) 

The n waypoints pj {x, y, z) , j — 1 ■■■n are the support points of the spline interpo- 
lation. A number of n points causes k = n—l segments. Each segment contains three 
spline functions including four spline parameters. To determine four parameters four 
equations are required. The following requirements for each segment boundary are 
defined: 

• Consecutive spline segments must be connected with each other. 

• The first and second derivatives must match the previous segment's derivatives. 

Note that path segment boundaries at a and b may have an enforced slope as well. 
Otherwise, the second derivatives at the boundaries a and b are set to zero. Figure[T| 
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Fig. 1 Example: Path based 
on splines supported by 
three waypoints, resulting 
in two segments, with un- 
specified boundaries at the 
coordinates (0,0) and (0,4). 




shows a path result without enforced slopes at a and b. The approach presented 
enables a smooth transition between the segments up to the second derivative. The 
parameter sets defined for each spline yield the coordinates between the knots. In 
addition to the desired overall path shape, vehicle configurations on that path must 
be determined. The following section focuses on this task. 



3 Trajectory Definition 

The three-dimensional path defined in the section above has to account for obstacles 
and to ensure the attainment of a set of global mission goals. Given a sufficiently 
low velocity, helicopters are able to fly along paths with arbitrarily sharp turns. Thus, 
solely the velocity is selected as the timely annotation of a path. 

The curvature of the path is used to determine the velocity. However, curvature 
changes may yield larger velocity changes than the vehicle can perform. Hence, a 
search for velocity minima along the path must be performed before the vehicle 
starts to fly the trajectory. From points, where the deceleration to the velocity min- 
imum has to begin, the velocity will be reduced while the vehicle still follows the 
given path. 

In this section first the determination of the velocity based on the curvature will 
follow. To adopt the velocity profile for the limited acceleration, a search algorithm 
for velocity minima will be presented afterwards. 



3.1 Quantification of the Velocity 

Based on the curvature k the velocity will be calculated for certain points T^+i 
Ti + 4 T. The curvature K for a space curve S{t) is defined at a point T as: 



,C(T) = 



1 



5'(t)x/(t) 



rcircleiT:) 



S{T) 



(9) 



Based on Tietze 0, the following relations will lead to the largest velocity com- 
mand based on the thrust available for a helicopter. The sum of the forces acting on 
the center of gravity is used to determine the ability to compensate for gravity and 
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centripetal force, and to change the velocity if desired. With the symbols Fjq for the 
gravity force vector, Fj^ for the aerodynamic force vector, F^ for the thrust vector 
of the helicopter and f ^^ for the inertial force vector the sum of the forces results in 

Q = LG + FA+F^ + Fji. (10) 

The main thrust is produced by the main rotor of the helicopter. Due to the small 
magnitude of the aerodynamic forces it is assumed that the magnitude of the thrust 
of the main rotor must equal the sum of gravity and inertial forces: 

\Fs\ = \Lk\ + \Lg\ (11) 

\F_s\ = \j{mYKf + ('«iV/f cos 7)2 + (mf/K)- + mg. (12) 

In eq. dTSt . m represents the vehicle mass, Va: the inertial velocity value, 7 and X 
the orientation of the path w. r. t. the geodetic frame, and g the magnitude of the 
acceleration due to the gravity force. 

Replacement of y by *[^°'^~^ and 7 by -r^ in eq. (fT2l l results in 

^circle ^circle 

ilEsl-'ngf - {mVKf + ^{mVif {cos''y+ l) . (13) 

circle 

The term -7 can be replaced with K^, cf. eq. Q. A definition of the largest acceler- 

F ■! 

ation Umax = ' which can be produced by thrust enables the elimination of the 

dependency on mass m. The velocity-curvature relation results in: 



Therefore, eq. ( fT4l i defines the largest velocity depending on the current acceleration 
in path direction and the curvature of the spline. 

Figure |2] shows a typical velocity profile for a spline curve over T. The solid 
(green) line represents the largest velocity limited to 20 m/s and is based on eq. 
( fT4l ). As explained in more details in the following subsection, the dotted (blue) 
line defines the velocity command for a deceleration with a specified deceleration 
limitation. 

Due to the neglected aerodynamic forces and the issue that the vehicle is not able 
to change the flight path arbitrarily fast a scaling factor will be introduced. Beside 
the flight velocity the path deviation must be provided to follow the path e. g. in the 
presence of disturbances. A trajectory following control system to guide the vehicle 
based on the desired path and the path deviation will be presented subsequently to 
the following subsection. 
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Fig. 2 Velocity profile for a 
spline curve. Based on the 
curvature the maximum ve- 
locity is determined (solid). 
Due to the deceleration lim- 
itation the dotted velocity 
profile is commanded to 
slow down to the velocity 
minima or stop at the end. 




Spline parameter 7 



3.2 Velocity Minima Search Algorithm 

In general, vehicles have acceleration and deceleration limitations. A sole path ge- 
ometry cannot account for such limitations. Thus, based on the largest velocity de- 
fined in eq. ( fT4l l the occurrence of minima, as illustrated in fig. |2l must be deter- 
mined. The first challenge is the search for a minimum since the curvature is only 
known at discrete points. Moreover, the velocity profile may have multiple local 
minima which self-denies the use of gradient based search algorithms only. 

Hence, locally bounded gradient searches are performed on a sampled spline. 
Changes in the slope of the velocity are used to detect minima. For each detected 
change a gradient based minimum search is performed. Similarly to the approach 
in 1 1 1 , the minima detection can be ensured by the property of resolution complete- 
ness that defines a sufficient distance between each sample and a sufficient number 
of samples for each path section. 

The search through the spline results in a table of velocity minima with respect 
to their corresponding position on the spline. Based on a model for the deceleration, 
a point for the initiation of a slow-down maneuver can be determined. This method 
gives a velocity command depending on the distance to a minimum. In this imple- 
mentation a constant velocity slope of 0.3 s^^ is used. By applying a method to 
convert distances into changes of T, cf. section |431 the table is supplemented with 
the specific slow-down points. Preliminary to the selection of the first entry a sort 
by the slow-down points results in a list, which has the crucial points in the right 
order. 

In this work this global search is enhanced to become a quasi incremental search. 
Similar to the approach in f4l|, a velocity minimum nearest to the helicopter position 
defines the initial search horizon w. r. t. T. This decreases the initial search time. 
The vehicle can start or continue flying while searching for minima that are further 
away. Of course, saving initial time comes at the price of additional computational 
effort during flight. Nevertheless, this becomes particularly useful when unforeseen 
obstacles require path re-planning: Low level flight paths through urban terrain may 
have a large set of velocity minima to account for. 
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4 Trajectory Following Control 

After the definition of a path a feasible trajectory was defined in the previous section. 
The aim of the trajectory following control system is to guide the vehicle to stay on 
the given trajectory. Due to unconsidered disturbances and simplifications made in 
the model used for trajectory generation, the vehicle will not follow the trajectory 
without error feedback. Therefore, a control system for trajectory error reduction 
must be designed. 

Based on the velocity profile a combination of feedforward and error feedback 
signals is used to constitute the desired velocity vector. In addition to the control gain 
determination for error compensation, the determination of the feedforward veloc- 
ity vector has major influence on the trajectory following performance. Moreover, 
as the spline-based path is nonlinear, the calculation of path errors is nontrivial. By 
expressing the path error in a path fixed frame and transforming the resulting veloc- 
ity vector afterwards into the geodetic frame the control problem can be simplified. 
The compensation of path errors will be done by proportional feedback. 



4.1 Definition of Frames and Transformations 

For the application for an UAV, used to demonstrate this method, some simplifica- 
tions are made. The operation in a local urban terrain allows the assumption of a 
flat Earth. Therefore, the position is described in a Cartesian coordinate system with 
respect to an initial point p^ = [xq, yo, zqY ■ From this origin to the current position 
in the geodetic frame (Index "g") the vector p represents the position of the UAV: 



X- 


-■^0 


y- 


-vo 


^ - 


-zo_ 



(15) 



A path-fixed frame (Index "k") is defined as a coordinate system where the Xj^ axis 
points into the direction of the flight velocity as depicted in fig. [3] The angles y and 
X specify the orientation of the yi^^ and Zk axes with respect to the geodetic frame. 



Fig. 3 Definition of frames 
and angles for a path fixed 
frame with respect to the 
geodetic frame, cf. Brock- 
haus ini 
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The transformation of a velocity vector represented in the Cartesian coordinate 
system into polar coordinates is defined as: 



Vk 
X 
7 



IVI 



atan2{vKg, UKg) 
( ''•Kg \ 

\hKg+''Kg\\) 



—atari 



with \\uKg + VKg\\^0, 



(16) 



where the two-argument function atanl returns the angle in radians between the 
positive MATg-axis of a plane and the point given by the coordinates {uKg,VKg) on it. 
A deviation from the path, e. g. due to a sudden change of wind or a gust, will 
be compensated by the feedback of the path error. These errors with respect to the 
path fixed frame Axi,, Ayi, and Azk are defined to be positive when the vehicle is 
left, behind and above the desired position. An error vector between a commanded 
position (Index "c") and the current position (Index "s") is defined as: 



Xc 


-Xs 


yc 


-ys 


_Zc 


-Zs_ 



^P.= 



The transformation of the error A p into the path-fixed frame is achieved by: 



(17) 



Ax 
Ay 

Az 



Tgk-^P,, T,k 



cosjcosx —sinx sinycosx 

cosysinx cosx sinysinx 

—siny cosy 



(18) 



4.2 Determination of the Velocity Command 

The first derivative of a spline, as defined in eq. Q, leads to a vector which rep- 
resents a tangent on the path for a given parameter T. The direction of this vector 
is used to generate a velocity vector, whose orientation can be expressed in terms 
of the angles x and y by using eq. ( IT6l ). The absolute value of the velocity Vc is 
determined based on the velocity profile described in section[3]and fig. 121 

The resulting velocity vector represents the command for a vehicle which is ex- 
actly on the desired path. To compensate path errors this vector must be modified to 
guide the vehicle back to the path. When expressing the path error in the path-fixed 
frame, as defined in eq. i fTSl ), the modified velocity command in the geodetic frame 
will be calculated with: 



TgkiXcTc) 



/ 


V, 




Axk 







+ K 


^yk 


v 





k 


_Azk_ 



,K 



'k, 0' 


ky 


k. 



(19) 



A path error Axk will therefore result in a larger velocity in path direction. Path er- 
rors Ayk and Azk will result in a rotated and in its absolute value modified velocity 
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command. With regard to the velocity limitation based on the formerly defined cri- 
teria the velocity command Vc should be less than VK.max from eq. (fT4b . The limited 
velocity is defined as: 

VKMm = SF-VK.,na„ SF<1. (20) 

The velocity command obtained from the velocity profile and the tangent on the path 
is scaled to V/f,/„„ with respect to its orientation. The values of the gains k^, ky and 
fc will define the response to path errors. Therefore their determination was done by 
adjusting the gains for a step in a (simulated) path error to result in an asymptotic 
error reduction with overshoot less than 3%. 



4.3 Determination of the Desired Position on a Spline 

The position of the vehicle can be determined based on sensors suitable for the 
applicatior^. However, to calculate the path error at a certain time the correspond- 
ing commanded position must be determined. For a spline, which is a piecewise 
parametric space curve, the position is expressed in terms of the formerly defined 
parameter T. Compared to a path length between two points a change in this param- 
eter has a nonlinear relation to a distance in the 3D space. In order to determine the 
desired position for an increasing T a method to convert between a distance in 3D 
and a change in the parameter T must be found. 

The length of a space curve can be obtained by its arc length. From a partition 
a = < Ti < ... < T„_i < T„ = b of the interval [a,b] we obtain a finite collec- 
tion of points p{to),p{ti),. . . ,p(t„_i),/7(t„) on the curve S. Denote the distance 
from p{Ti) to /7(t,+ i) by d{p{ti),p{ti+i)), which is the length of the line segment 
connecting the two points. The arc length / of 5 is then defined to be 

n-l 

m^ sup J^dipiTi),piTi+i)). (21) 

n=To<Ti< ■•<r„=&,=o 

By taking two points of a spline in terms of !„ and T^ the length can be approximated 
by using a step size controlled ordinary differential equation (ode) solver. The solver 
is supported by the first derivatives as well as an accuracy e which must be defined. 
For this application a Runge-Kutta solver with adaptive step size control was chosen. 
An initial step size of 10^^ and an accuracy of e = 10^^ turned out to be suitable. 

Moreover, the conversion of a distance into an appropriate change of the spline 
parameter T is done by using a combination of bisection and Newton-Raphson 
search algorithm. With the support of the length approximation method discussed 
before the search algorithm returns the spline parameter t ~ Tq + As correspond- 
ing to an initial To and a distance As. While Newton-Raphson's global convergence 
properties are poor, fail-safe routines exist that utilize a combination of bisection 



Usually GPS-aided sensor fusion algorithms can achieve position estimates of submeter 
accuracy. 
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and Newton-Raphson fT2l. The hybrid algorithm takes a bisection step whenever 
Newton-Raphson would take the solution out of bounds, or whenever Newton- 
Raphson is not reducing the size of the brackets rapidly enough. 

Based on the value of the commanded velocity Vc and the time At, elapsed since 
the last update, the passed distance As is approximated by 



As^Vc-At. 



(22) 



Any error that occurs due to this approximation results in a change in the desired 
position on the path. Therefore, by feeding back the error between the position of 
the vehicle and the desired position, this error will not have significant influence as 
long as the update time will be sufficiently short. 

The velocity profile shown in fig. |2] does not account for limitations in the ac- 
celeration (only for deceleration). Hence the path error in path direction zix^ would 
increase reasonably fast due to the limitations of the vehicle. Instead of accounting 
for this property in the trajectory definition, the modification of the reference po- 
sition on the path by extending eq. ll22l) was selected. Depending on the distance 
error Axi^ the change of the reference point T is reduced. By an additional design 
parameter K,- the progress of the reference point is controlled: 



As = Vc ■ At - Kr ■ Axi^. 



(23) 



Prior to the conversion of As to T, As is limited to 10 m. The design parameter 
K, is adjusted to 3 % overshoot. Finally, by using T in eq. Q, the desired position 
command p can be provided. 

This path control system was validated in real-time simulations and flight tests. 
The algorithms turned out to be a feasible solution. However, in case of a large 
curvature the velocity provided by eq. (fT4b will be small. Note that the velocity 
command can become infeasible as soon as it is in the order of the velocity state 
estimation noise. Moreover, this effect can be reinforced by the limitation as 




Fig. 4 Flight test result for a spline based trajectory 
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presented in eq. ( l20l ). Thus, a minimal velocity is enforced until the end of the path 
is reached. 

For the application to a helicopter UAV, the path tracking showed an acceptable 
path following performance. However, a systematic path error is observable (figHJl 
and thus needs further investigation. One reason for this effect seems to be the time 
the helicopter needs to adjust to the new velocity command. Due to the constantly 
changing command for the velocity, the flight state lags behind. Moreover, the path 
error compensator appears to be not fast enough to compensate this error. Thus, 
research is underway to prevent this lagging. 



5 Summary 

This work presents a decoupled trajectory generation approach for motion planning. 
Cubic spline segments are used to generate a collision-free path through narrow 
passages. Based on the curvature of the path the velocity profile is defined. This 
way, the complex nonlinear dynamics of a VTOL vehicle can be separated from 
geometric path planning and trajectory generation. The system has been success- 
fully flight tested on an unmanned helicopter. 

Motivated by the physical model of the vehicle, the trajectory curvature and 
the vehicle acceleration limits are used to compute the maximum velocity along 
the trajectory. To account for path changes during flight (e. g. obstacle avoidance) 
the computational efficiency for replanning is addressed by incrementally determin- 
ing the velocity profile. It is updated incrementally towards a virtual horizon moving 
along the trajectory. 

Furthermore, a trajectory following system is presented which utilizes instanta- 
neous trajectory tangents to generate a feasible velocity command vector for the 
control system. It compensates potential path errors, e. g. due to wind disturbance. 
Finding the desired position on the path is a nontrivial task and is caused by the 
parameterization of the space curve by a knot vector. Thus, a conversion method for 
the arc length of a curve and a change of the spline parameters is proposed. More- 
over, a linear control law is used for path error compensation. To prevent exceeding 
the maximum velocity limit, a scale factor reduces the velocity command. 

Further research will have to focus on a reduction of the path deviation. This 
reduces worst case safety margins needed for path collision checking. 
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A Linear Parameter Varying Controller for a 
Re-entry Vehicle Benchmark* 



Andres Marcos and Samir Bennani 



Abstract. In this article the design of a linear parameter varying controller for an 
atmospheric re-entry vehicle benchmark is presented. The control design approach 
used is based on the Single Quadratic Lyapunov Function approach. The re-entry 
vehicle used is a high-fidelity benchmark that includes full nonlinear motion, de- 
tailed aerodynamic database, nonlinear actuators, colored sensor models, realistic 
uncertainties and a control-surface mix logic. The latter logic fully couples the 
longitudinal and lateral/directional motions and together with the noise and uncer- 
tainties used result in a challenging and representative atmospheric re-entry 
benchmark. The results indicate that the LPV controller satisfies all the perform- 
ance and robustness objectives and alleviates the designer task due to the auto- 
mated gain-scheduled nature of the approach. 



1 Introduction 

Gain scheduling [1, 16, 17] is probably the most widespread approach in the 
Space industry to design controllers for systems undergoing large dynamical 
changes or requiring complex mode switching implementations. It can be consid- 
ered an incremental step to local synthesis that uses linear time invariant (LTI) 
point-designs together with standard gain scheduling concepts, such as interpola- 
tion, to obtain a global controller based on the local designs. Despite its wide 
spread use there are some drawbacks due to its ad hoc validation character, mainly 
related to the effect that the selection of the design points and of the scheduling 
rule (in terms of parameters and complexity) has on the performance of the global 
gain-scheduled controller. 

Linear parameter varying (LPV) synthesis techniques have positioned themselves 
as an alternative, or more advanced, step to gain scheduling and can be considered 
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as a type of automated gain scheduling approach [1, 18]. These techniques are at the 
center of a European Space Agency (ESA) study established to address an envi- 
sioned need for advanced gain scheduling techniques in Space. The "LPV Modeling, 
Analysis and Design (LPVMAD)" consortium led by Deimos Space (Spain) and 
composed by research teams from the Computer and Automation Research Institute 
(SZATKI, Hungary), Delft University of Technology (The Netherlands) and Leices- 
ter University (United Kingdom) was formed to address the development of an in- 
dustrial LPV control design framework supported by reliable LPV software tools. 
Specifically, the objectives of the study were: 

1 . To assess the possibility, needs and impact of LPV techniques in the control 
design process for space systems 

2. To propose a control design LPV framework 

3. To develop reliable LPV tools for modeling, analysis and design in support of 
such a framework 

4. To demonstrate the developed framework and tools for a relevant space system 

Results from Phase I of the study [10, 11, 12, 13, 14, 15] detailed the successful ac- 
complishment of the first three points above. Phase II was tasked with addressing 
the 4th point above using a more challenging benchmark that included practical is- 
sues such as saturation, motion coupling, controller scheduling and time-varying 
behaviour. The selected benchmark was an enhanced version of the well-known 
NASA HL-20 atmospheric re-entry vehicle. 

This article presents results from the fourth objective of the project. In particu- 
lar, it details the design of an LPV controller for the low supersonic to subsonic 
phase of the selected re-entry benchmark. The layout of the article is as follows. 
Section 2 presents the LPVMAD re-entry benchmark. Section 3 details the de- 
coupled longitudinal and lateral/directional LPV controller designs. Section 4 pre- 
sents the analyses performed during the design cycle as well as the final validation 
campaign, based on a Monte Carlo campaign using the full, i.e. coupled, nonlinear 
vehicle under parametric (center of gravity, moments of inertia, mass and aerody- 
namic coefficients) and time-varying (Mach number) changes. Section 5 con- 
cludes with a summary of the results. 

2 LPVIVIAD Re-entry Benchmark 

The NASA HL-20 lifting-body vehicle was proposed as a substitute of the Space 
Shuttle Orbiter and although it was finally de-commissioned, many research ef- 
forts were performed to develop a fairly detailed baseline aerodynamic database 
for the complete velocity range of the vehicle [2, 3, 4, 5]. 

For the present study, a 3 Degrees-of-Freedom optimized guidance trajectory 
adapted from reference [7] is considered. The study focuses in the pure aerody- 
namic surfaces phase corresponding, approximately from low supersonic to 
sub-sonic speeds, i.e. Mache [3,0.8]. This selected phase is characterized by the 
end of a bank reversal and a similar maneuver for the angle-of-attack, see Figure 1. 
Notice that these almost simultaneous maneuvers results in strong coupling of the 
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longitudinal and lateral/directional motions (especially severe due to the control 
surface mix logic contained in the benchmark -more on this below). 




1780 
time (sec) 



teao 




1780 
time (sec) 



1880 



Fig. 1 Reference trajectory: angle-of-attack and bank angle versus time 



The full nonlinear equations of motion, see reference [2] for details, are used 
together with a representative aerodynamic database [2, 3, 4, 5]. This HL-20 im- 
plementation is a quite more advanced, and representative, model than the pub- 
licly available Matlab model [6], which uses the polynomial simplifications given 
in NASA report [3]. The complete aerodynamic database is formed by nonlinear 
look-up tables (LUT) dependent on Mach number, angle of attack, sideslip and 
control surface deflections. 

The available physical control surfaces are upper left and right flaps (DUL and 
DUR), lower left and right flaps (DLL and DLR), wing left and right flaps (DEL 
and DER) and rudder (DR). The deflection is positive downward for the up- 
per/lower flaps (h-TED and -TEU), away from the vehicle for the wing flaps 
(h-TED) and to the left for the rudder (-hTEL). The system implements a nonlinear 
control surface mix logic that transforms the controller-commanded elevator S^u, 
aileron Sau, speed-brake Ssbk and rudder dmd deflections into the previous physical 
surfaces based on Mach number and complex nonlinear relationships. The output 
of the control-mixer is passed to the actuation system. Each actuation system is 
composed of a series interconnection of a second order filter, a magnitude limiter, 
a rate limiter and a time delay (of 0.005 seconds). The sensors are implemented as 
first order coloring filters and include measurement bias. 

A multiplicative uncertainty model is considered around nominal values of the 
parameters u, based on given percentage uncertainty ranges A% and normalized 
random but bounded gain Su, i.e. u^=u(l+ SuA%). For those magnitudes with 
nominal value equal to zero, an additive model is employed (the multiplicative 
model would not introduce uncertainty in the parameter for these cases). Refer- 
ence [19] details the nominal values and percentage uncertainty A% used in the 
re-entry vehicle benchmark for the main vehicle geometry and aerodynamic pa- 
rameters: center of gravity (e.g. A% of +0.5% for Xcg), moments of inertia (+5%), 
mass (+5%) and aerodynamic coefficients -including a Cl/d uncertainty profile 
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Fig. 2 Basic aerodynamic coefficients in term of angle-of-attack and Mach 

that serves to physically relate the uncertainties from Cl and Co. Further, the aero- 
dynamic database is quite nonlinear and alternates stable with unstable areas - no- 
tice the positive and negative slopes in Cmo from Figure 2. 

A detailed analysis of the dynamic characteristics of the benchmark is given in 
[9, 19], and shows that the vehicle is quite challenging with shifts in stability for 
the lateral/directional and longitudinal motions and strong natural frequency and 
damping changes as the vehicles flies the trajectory, see for example Figure 3. 



-a- 



Region: 2.25 >= Mach < 3.0 
Region: 1.74 => Mach < 2.25 
Region: 1.05 => Mach < 1.74 
Region: Mach < 1.05 



-0.8 



-0.4 
Real Axis 



Fig. 3 Lateral/Directional motion: Pole/Zero map based on Mach regions 



3 LPV Control Design 



This section presents the application of advanced gain-scheduling control design 
methods to the presented high-fidelity benchmark. The LPV technique used is the 



A Linear Parameter Varying Controller for a Re-entry Vehicle Benchmark 19 

so-called Single Quadratic Lyapunov Function (SQLF) approach [18]. Essentially, 
this technique synthesizes a LPV controller by means of finding a single quadratic 
Lyapunov function, not depending on the varying parameter vector, that satisfies 
an LPV version of the well-known bounded real lemma (BRL). In practice, it 
searches for a Lyapunov function valid for a set of linear matrix inequalities 
(LMIs) corresponding to the satisfaction of the BRL at a grid of values of the 
varying parameter vector that defines the LPV plant. 

The principal advantage of this method is that it builds up from LTI Ha, synthe- 
sis. And actually, the current software tools used to synthesize the LPV controller 
directly takes the weights and interconnection used in the standard LTI H„ design 
set-up and facilitates its tuning to arrive at the desired global performance. Indeed, 
the synthesis of design-point LTI H^, controllers can be seen as a first step in the 
application of this LPV technique. LTI H„ optimization is a design technique 
where specification of performance and robustness objectives is the main driver in 
correctly posing the mathematical optimization problem -as opposed to other con- 
trol synthesis techniques where satisfaction of the objectives is evaluated after the 
design. LTI H„ control synthesis is one of the cornerstones of modern control and 
is widely used across industry [16, 17, 18]. Thus, in order to assess the advantages 
and shortcomings of the advanced LPV control solution, a standard gain- 
scheduling control design based on LTI Hoo design-point controller can be used. 
Such a baseline controller was developed and presented in reference [19]. Since 
this baseline controller follows a decoupled motion design approach, the LPV de- 
sign process also uses a motion decoupling approach during synthesis -but both 
are always validated using the full, i.e. coupled, nonlinear vehicle. 

The control design objectives are to track the reference angles of attack, side- 
slip and bank in the face of the established noise and uncertain levels with desired 
deviations of less than 2 degrees, and with acceptable short-term deviations of less 
than 4 degrees. 

A. Longitudinal Motion Controller Design 

The design rationale for the longitudinal motion is that of angle-of-attack tracking 
through an ideal-model formulation. The same design rationale and plant set-up as 
for the baseline control design are used [19]. 

Since only inner-loop control is considered, the open-loop plant can be reduced 
to the short-period motion. In this manner, the state dimension of the controller is 
largely reduced (recall that for LTI Hoo-based synthesis the obtained controller has 
the same number of states as the design interconnection). The open-loop plant 
used for design has 2 states (angle-of-attack and pitch rate), two outputs (same as 
the states) and two inputs (elevator and speed-brake deflections). The defined H^ 
design interconnection is given in Figure 4. Wact penalizes the actuators magni- 
tude; Wrob is used to add uncertainty at the plant input; Wperf and Wid serve to 
capture the angle-of-attack tracking formulation and to penalize its error; Wcmd 
shapes the angle-of-attack input; and finally, Wnoise is used for robustness by 
shaping the noise effect on the system. The /-block is introduced to indicate that 
no actuator model is used for the longitudinal motion. 
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Fig. 4 LPV controller design interconnection 

The only modification with respect to the baseline control design [19] is the use 
of a more refined grid of LTI plants around which to synthesize the LPV control- 
ler and also a slight change in the design weights. There is an apparent increase of 
complexity, four different sets of weights are used for the LPV controller design 
versus three for the baseline, but the resulting LPV controller is indeed a single 
dynamic system and no effort on ad hoc manual gain-scheduling is required - 
which is after all the greatest advantage of LPV synthesis techniques. Table 1 
shows the weights for the four longitudinal design-point controllers in terms of the 
generic weight transfer function Weight = K(as+1) / (bs+1). 

Table 1 Longitudinal LPV controller: H„ weights per Mach region 
[ - := same weight as on the Mach region to the left; d2r:=180/7l ] 
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B. Lateral/Directional Motion Controller Design 

The lateral/directional LPV controller is more complex than the previous LPV 
longitudinal controller, which is a single controller obtained using 4 sets of 
weights. Nevertheless, the use of the LPV synthesis approach simplifies the global 
lateral/directional controller complexity as for the LPV case is composed by two 
different LPV controllers straightforwardly "blended" (i.e. linearly interpolated) in 
contrasts to three different controllers for the baseline control design [19]. 

The lateral/directional open-loop plants used for the design consist of four 
states (yaw rate r, roll rate p, sideslip angle P and bank angle a), two input chan- 
nels (aileron dau and rudder 5n,d deflections) and four outputs (yaw rate, roll rate, 
lateral acceleration n,. and bank angle). The interconnection used is as in Figure 4 
but with the weights based on a model-matching approach for bank angle com- 
mands (Wcmd and Wid) and including two additional objectives in the perform- 
ance weight Wperf: lateral acceleration minimization and turn-coordination [8]. 



Table 2 Lateral/Directional LPV controller: Hoc weight per Mach region 
[ - := same weight as on the Mach region to the left; d2r:=180/7l ] 



Type 


Weight 
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Additionally, and critical in tiiis case, it is the inclusion of magnitude and rate 
first-order models for the actuator weights (in the position of the /-block)-Table 2. 
From a qualitative aspect, and based on a comparison with the experience of 
designing the baseline controller [19], it is remarked that the tuning of the weights 
can be performed much faster for the LPV controller than for the baseline due to 
the global automated scheduling nature of the approach. That is, the LPV synthe- 
sis adds an additional design degree of freedom through the optimized search of a 
common Lyapunov function valid at the selected grid points. This optimized 
search compensates for any 'rough' weight definition at the local points; alterna- 
tively, this can be seen as allowing the engineer to focus on performance when de- 
fining the weights at the local points while robustness is achieved by the auto- 
mated scheduling of the LPV approach. It is worth mentioning that the use of the 
final LPV weights for the redesign of the ad-hoc gain-scheduling does not result in 
similar performance and robustness properties. This is the result of the non-global 
nature of the ad hoc gain-scheduling and the manual selection of the interpolation 
scheme. In summary, the systematic character of the LPV synthesis techniques 
can save a lot of time in defining the weights and in achieving the required control 
design objectives when compared to traditional ad-hoc gain-scheduling. 



4 Control Analysis 

A. Linear (During-Design) Analysis 

During the design phase a set of linear analytical analysis and pseudo-linear time- 
simulations (linear but with some selected nonlinearities) are performed to con- 
verge to a controller that fulfills the desired objectives. Among these analyses: 

• Weighted performance versus inverse of the performance weight. 

• Weighted actuation versus noise and inverse of the actuation weight. 

• Controller GK transfer functions for each of the tested plants. 

• Sensitivity transfer functions and flying qualities. 

• Time-domain simulation using the LTI plants and corresponding extracted LTI 
controllers together nonlinear actuators (dynamics, rate and magnitude satura- 
tions) and sensors (noise and dynamics). 

A summary of results is given in this section. For example, Figure 5 shows the lon- 
gitudinal weighted and controller transfer functions (TFs). As it is seen, they are 
below the inverse of the weights which means that the controller is unaffected by 
the corresponding noise channels and have the appropriate shape. Figure 6 on the 
other hand shows longitudinal controller linear step response simulations, which 
serve to evaluate its flying qualities (this is tested for a set of LTI plants including 
those used for design as well as other points in the flight envelope). 

This is then validated through pseudo-linear (i.e. linear plant with selected 
nonlinearities) time domain simulations. Figure 7. A doublet-plus-ramp profile for 
the angle of attack reference command is used to determine the controller tracking 
of step and ramp profiles. This is especially important in the benchmark as it is 
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noticed that good tracking for steady-state commands (e.g. step responses) is eas- 
ily achievable in the full nonlinear simulation but that ramp-type profiles, which 
are very characteristics for unpowered vehicles in approach configuration, is more 
difficult to achieve while satisfying actuation limits. The results in Figure 7 shows 
that for the linear plus nonlinear actuation/sensor, the controller is valid -still, note 
the increase in elevator actuation for the ramp tracking. 
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Fig. 7 Longitudinal LPV controller; pseudo-linear responses (LTI plant with full actuators 
& sensors) 



With respect to the lateral/directional controller, the analyses performed during 
the design process also show the satisfaction of the objectives. Nevertheless, in 
this case it is observed some noise influence at high frequencies -see Figure 8 for 
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Fig. 10 Lateral/Direction LPV controller: pseudo-linear responses (LTI plant with full ac- 
tuators & sensors) 



an example using the weighted actuation linear analysis. Although this coupling is 
not critical it is reflected in the sensitivity analysis (S and T) and in the time- 
domain simulations, see Figure 9 and Figure 10. Thus, the conclusion from the 
analyses is that the lateral/direction LPV controller has the desired frequency and 
flying quality characteristics and should fulfill, in as much as linear analyses can 
guarantee, the objectives in the nonlinear simulation. 

In summary, for both LPV controllers it can be seen from the linear analytical 
analysis perspective, that all objectives are satisfied. In the following section, it is 
shown that the conclusion from these during-design analyses is validated in the 
full nonlinear time-domain simulations through a Monte Carlo campaign. 
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B. Nonlinear Full-Motion Monte Carlo Analysis 

A Monte Carlo campaign is performed using the full-motion nonlinear simulator 
of the high-fidelity benchmark and the uncertainty parameters and ranges defined 
in Section 2, see also [9, 19]. No failed cases, out of >1400 runs, are found indi- 
cating that the LPV controller achieves the desired robust objectives while all er- 
rors are within the limits (see Figure 11) indicating that the performance objectives 
are also achieved. When compared with the results from the baseline controller - 
not shown due to space limitations, see [19] — the LPV controller compares very 
well and improves both performance and robustness. 
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Fig. 11 Monte Carlo full-motion: tracking signals and errors 



5 Conclusion 



In this article, longitudinal and lateral/directional LPV controllers have been de- 
signed and analyzed for a high-fidelity re-entry benchmark. The LPV design uses 
the so-called Single Quadratic Lyapunov Function (SQLF) approach, which builds 
up on LTl H„ point-designs. The assessment of the full-motion LPV controller has 
been performed using a wide array of linear analytical techniques (from sensitivity 
transfer functions to flying quality responses) and full nonlinear time-domain 
Monte Carlo campaigns. The results indicated that that LPV controller designs 
fulfill all the performance and robustness objectives. Further, in comparison to a 
traditional gain-scheduling controller [19], the systematic character of the LPV 
synthesis techniques can save a lot of time in defining the weights and in achiev- 
ing the required control design objectives. 
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A Low Cost Small UAV Flight Research Facility 

Austin M. Murch, Yew Chai Paw, Rohit Pandita, Zhefeng Li, and Gary J. Balas 



Abstract. This paper presents an overview of the low-cost, open source small Un- 
manned Aerial Vehicle (UAV) flight research facility at the University of Minnesota. 
A detailed description of the facility, its components, and its capabilities is pre- 
sented, as well as applications of the UAV platforms to research, education, and 
collaboration. Flight results from controls research is presented, followed by a brief 
summary of current research and future directions. 



1 Introduction 

Unmanned Aerial Vehicles (UAVs) are used worldwide today for a broad range of 
military, civil, and research applications. There continues to be a growing demand 
for reliable and low cost UAV systems. This is especially true for small to miniature- 
sized UAV systems (less than 2 meter wing span) where the majority of systems are 
still deployed as prototypes due to demand and lack of reliability. Improvements in 
the modeling, testing and flight control for these vehicles would help to increase 
their reliability and performance of small UAVs during operations. The traditional 
approach used in the development cycle |ITJ[2l for manned aircraft is time consum- 
ing and resource intensive. Applying the same techniques to the small UAVs is not 
realistic. 

The UAV Research Group in the Department of Aerospace Engineering and Me- 
chanics (AEM) at the Univeristy of Minnesota is focused on development and im- 
plementation of a low-cost, open source small Unmanned Aerial Vehicle (UAV) 
flight research facility. The goal of this facility is to support research activities within 
the department including control, navigation and guidance algorithms, embedded 
fault detection methods, and system identification tools. The system is built mostly 
out of commercial-off-the-shelf (COTS) components to minimize the overall mate- 
rial and development costs. In addition, the entire architecture is open and available 
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to any researchers or organizations who wish to collaborate on the development or 
application of the UAV capabilities, (http://www.aem.umn. edu/^uav/) In addition 
to the researchers from AEM department, the UAV Research Group is collaborat- 
ing with researchers at the Budapest University of Technology and Economics in 
Hungary and the University of Sannio at Benevento, Italy. 

2 UAVTestbeds 

The UAV Research Group uses COTS R/C fixed wing aircraft modified to carry the 
necessary avionics and instrumentation payloads. Several different aircraft models 
have been used during the development process, but the primary test aircraft is the 
Ultra Stick 25e fSl (shown in Figure [T(a)j l. Two additional aircraft in use are an Ultra 
Stick 120 (shown in Figure [T(b)j l which can handle significantly more payload than 
the Ultra Stick 25e, and the Mini Ultra Stick, the smallest version of the Ultra Stick 
family. 




(a) Ultra Stick 25e 
Fig. 1 Ultra Stick UAV Testbeds 



(b) Ultra Stick 120 (FASER) 



The Ultra Stick 120 (aka FASER; Free-flying Aircraft for Subscale Experimen- 
tal Research) has a conventional horizontal and vertical tail with rudder and ele- 
vator control surfaces, respectively. The aircraft has a symmetric airfoil wing with 
aileron and flap control surfaces. All six control surfaces are actuated by Hitec HS- 
5625MG servos. The plane is propelled by a 1900W Actro 40-4 brushless electric 
motor with a Graupner 14 x 9.5 folding propeller. Power for the motor comes from 
two SOOOmAh 5-cell lithium polymer batteries connected in series. The servos are 
powered by a separate 1350 mAh 3-cell lithium polymer battery. The main internal 
payload bay is located directly under the wing in the fuselage and measures approx- 
imately 35cm L X 10cm H x 10cm W; additional payloads may be accomodated 
in the aft fuselage or externally. The 120 class can carry approximately 2.5kg of 
payload. 

The Ultra Stick 25e is an approximately 65% scale model of the Ultra Stick 120, 
with the same basic configuration. The UAV Research Group maintains three Ultra 
Stick 25e aircraft, named 'Odin', 'Loki', and 'Thor'. All six control surfaces are ac- 
tuated by Hitec HS-225BB servos. The plane is propelled by a 600W E-Flite Power 
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Table 1 Summary of Aircraft Parameters 



Parameter 


Mini Ultra Stick Ultra Stick 25e 


FASER 


Wing span 


0.985 ;n 


1.27 m 


1.92m 


Wing chord 


0.21m 


0.3m 


0.43 m 


Length 


0.865 m 


1.05 m 


1.32m 


Wing reference area 


0.21 ra2 


0.32 m2 


0.769 in^ 


MTOW (Tested) 


N/A 


2.04 kg 


9.01kg 


Empty weight 


0.62kg 


1.50 kg 


6.35 kg 


Endurance 


10- 15 min 


15 -20 min 


15 -20 min 


Cruise speed 


10 - 15 m/ sec 


l5-20m/sec 


20 -30m/ sec 



25 brushless electric motor with an APC 12x6 propeller. Power for the motor and 
servos comes from a 4200mAh 3-cell lithium polymer battery. The main internal 
payload bay is located directly under the wing in the fuselage and measures ap- 
proximately 22cm L X 6cm H x 7.5 cm W; additional pay loads may be accomodated 
in the aft fuselage or externally. The 25e class can carry approximately 0.55kg of 
payload. 

The Mini Ultra Stick is an approximately 50% scale model of the Ultra Stick 
120, with the same basic configuration. The UAV Research Group currently uses 
this aircraft as a wind tunnel model, as the smaller size enable it to fit into the AEM 
department's low speed wind tunnel. 

The specifications of the three Ultra Stick aircraft are given in Table[Tl The Ultra 
Stick aircraft family is marketed as a trainer-level aerobatic aircraft, so it is relatively 
easy for a moderately skilled R/C pilot to fly, but is still capable of highly dynamic 
manuevering flight. 



3 Onboard Avionics 

The current architecture of the onboard avionics is shown in Figure |2] and Table [2] 
gives a listing of the individual components in the system. The flight computer uses 
a real-time operating system and flight software written in C. The flight computer 
handles data collection from each sensor, performs attitude and position estimation, 
executes flight control algorithms, stores relevant data, outputs PWM servo com- 
mands, and sends information to the ground control station via the data modem. 
A failsafe switching board is used to switch control of the aircraft between man- 
ual mode (human R/C pilot) and the flight computer. The hardware interface to the 
flight computer is handled via a custom-designed interface board. 

3.1 Sensors 

The sensor suite for the UAVs is focused on measuring the aircraft state data needed 
for normal flight guidance, navigation, and control algorithms. Position, velocity. 
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Fig. 2 Onboard avionics arcliitecture 



Table 2 Onboard avionics system components 



Component Module 



Cost 



Flight computer Phytec MPC5200B microcontroller $300 
Interface board AEM custom design $250 

IMU Analog Devices iSensor ADIS 16405 $800 

GPS USGlobalSat EM-406A $50 

Pressures Honeywell ASDX $60 

Data Telemetry Free Wave MM2 900 Mhz modem $375 
Failsafe Switch AcroName RxMux $300 

Manual Control Spektrum DX-7 2.4 Ghz R/C system $300 



and accelerations in the aircraft body frame and navigation frame are the normal 
quantities of interest. In general, the minimum sensor suite to achieve these data 
with sufficient accuracy is the combination of an IMU, GPS, and pitot-static system. 
Prior work has utilized an integrated sensor suite (Crossbow Micronav) that com- 
bined all of these functions, but this product is no longer available or supported. An 
affordable replacement for the Micronav has been difficult to find, and as a result, 
individual sensor components have been selected and integrated into the system. 
While this process does incur development and testing overhead, it has the advan- 
tage of allowing the researcher to select optimal sensors for the given application, 
and also offers a simple path for upgrading sensor capabilities. The replacement 
sensor components are described as follows: 

IMU sensor: The Analog Devices iSensor ADIS 16405 is a small, low-cost, tem- 
perature compensated, tri-axial accelerometer, rate gyro, and magnetometer. Data is 
provided to the flight computer via SPI bus at 50Hz. 
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GPS module: The USGlobalSat EM-406A is a small 25mm square unit with 
an integrated antenna. The GPS circuitry is located directly underneath the patch 
antenna, allowing a very compact operation. The EM-406A uses the SiRF Starlll 
GPS engine and data is provided to the flight computer via TTL serial at IHz. 

Pressure sensors: The Honeywell ASDX is a small IC-based, digital output, 
temperature-compensated pressure sensor available in differential and absolute ver- 
sions. These units are used to measure total and static pressure to determine the 
airspeed and altitude of the aircraft, and are connected to a Pitot-static probe that 
protrudes forward of the right wing of the aircraft. Pressure data is provided to the 
flight computer via I2C bus at 50Hz. 

3.2 Flight Computer 

The current flight computer is a phyCore MPC5200B-tiny 32-bit PowerPC micro- 
controller. Prior work has utilized an phyCore MPC55, but limitations in processing 
power motivated an upgrade. The MPC5200B has a clock frequency of 400 Mhz, 
760MIPS of processing power, and performs floating point computation. Current 
flight software utilizes about 2% of the CPU capacity. It has a wide range of I/O 
capabilities to support communication with external devices in addition to onboard 
data storage capacity. Details on the specifications of the MPC5200B can be found 
on the Phytec website yj. Data is stored onboard the MPC5200B in the 64MB 
SRAM and downloaded after the flight via an Ethernet connection to the ground 
station. 

3.2.1 Flight Software 

The onboard flight computer utilizes a real-time operating system (eCos) and flight 
software written in C. In addition to being open source and freely available, the 
eCos operating system provides a real-time kernel, can be configured to minimize 
the computing overhead required for the operating system, supports multi-threading, 
and is POSIX C compatible 13. 

The flight software uses multiple prioritized threads running at different rates 
to perform the necessary tasks for flight. Prioritizing multiple threads helps to en- 
sure the most important time critical tasks are performed first (e.g. data acquisition). 

Table 3 Flight software thread description 



Priority 


Thread 


Description 


Frequency (Hz) 


1 


DAQ 


data acquisition for each sensor component 


50 


2 


AHRS 


attitude determination using EKE 


50 


3 


CLAW 


flight control law and actuator commands 


25 


4 


INS-GPS 


INS/GPS navigation filtering algorithms 


10 


5 


DATA 


onboard data storage 


50 


6 


TELE 


packing and sending of telemetry data 


20 
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Currently, there are 6 threads, Usted in Table [3] which describes their function, pri- 
ority, and update rate. 

The data acquisition, data storage, and telemetry threads are relatively stable and 
few changes are made to these software functions from flight to flight. Most research 
activity occurs in the AHRS, flight control law, and INS/GPS software functions. 

4 Ground Control Station 

The Ground Control Station (GCS) is used during flight testing to monitor the 
aircraft state and health status. It consists of a laptop computer running the GCS 
software connected via serial to a data modem. The GCS software is a Java-based 
program inspired by the Open Source Glass Cockpit Project. It is designed to give 
vital flight information in real time to observers in order to assess the flight perfor- 
mance and maintain situational awareness of the aircraft during the flight test. The 
GCS software includes a Heads-Up Display, a moving map showing the location of 
the aircraft, commanded actuator positions, and indicators to display flight control 
mode information. Figure |4] shows a screenshot of the GCS software. 
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Fig. 3 Ground control station 



5 Simulation Testing 

The use of simulation-based development and testing prior to actual flight testing 
reduces the total development time and helps ensure the algorithm under develop- 
ment is validated and bug-free, reducing costly debug time in the field and minimiz- 
ing risk to the UAV platforms. The UAV Research Group maintains an integrated 
framework of three simulation environments used during the development pro- 
cess (Figure lU. The UAV simulation model is constructed in the Matlab/Simulink 



A Low Cost Small UAV Flight Research Facility 



35 



© 



^ © 



^© 



^(D 



Initial design testing : Software-in-loop testing : Processor-iti-loop testing 



Flight testing 



Linear/ 

nonlinear 

simulation model 



Monlinear 

simjJalion 

model 



Nonlinear 

simulation 

model 



Actual vehicle 



Coniraller 
implemented in 

Simutlhk block 



Controller 
implemented in C 
codes embedded 
in S-lunction blocK 



Controller 
implemented in C 

codes execute in 
actual processor 



Controller 
implemented in C 
codes execute in 
aolLial processor 



L 



Tfie' SIL rs Co Le^C ihs i::(Hrei;EnsS3 at 
dgsigried r.Qji]Tal}er iinpteiTienled ih C 
Mdes wiihirntio Matfab environinont. 



JL 



Ths PIL \s la is3\ linpieiTieriled codes 
QonlTaQHr IrOm SIL curtrung on IheacJUiil 

d pracesMr. 



T 



Tiw imptemenled C codes (rorn PIL is 
valkiatfld in rs^ Flight [B&lijig. This aisa 
h&ip EQ vi^NFy and update Iho nonlerxi^ 
simulation modf^l used in Ihe sfmulaKH'. 



Fig. 4 Application of simulation testing for flight control development 

environment using the Aerospace Blockset ^. All three simulations share the same 
nonlinear dynamics model. 

5.7 Nonlinear Simulation 

The 6-DOF nonlinear simulation model uses the full nonlinear equations of motion, 
linear derivative aerodynamics, and table lookup propulsion models. Models of rel- 
evant aircraft subsystems such as actuators, motor, propeller, sensor dynamics, and 
noise are included. The environmental model includes a detailed model of Earth's 
atmosphere, gravity, magnetic field, wind, and turbulence. The aerodynamic deriv- 
iatives were derived from first principles and empirical methods, and then updated 
using flight test data [Tj. Bifilar swing tests were used to determine the moments 
of inertia, and wind tunnel testing was used to characterize the motor and propeller 
thrust, torque, and power. 

5.2 Software-in-Loop Simulation 

The Software-in-the-Loop (SIL) simulation contains the nonlinear simulation with 
the C-code implementation of the control law in a Simulink S-function block. Only 
the flight control law is used at this stage; the remainder of the flight software is not 
included. This step is primarily used for debugging and to verify the C implementa- 
tion of the algorithm under development matches the designer's expectations. 



5.3 Processor-in-Loop Simulation 

The PIL simulation is an extension of SIL simulation that includes the MPC5200B 
flight computer into the simulation setup. Figure |5] shows the differences between 
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Fig. 5 Block structure of SIL and PIL simulation 



the SIL and PIL simulations. The Mathwork's Real-Time Windows Target toolbox 
ISJI is used to ensure the simulation runs in real time on a Windows PC which is 
crucial for meaningful results when the real-time flight computer is included in the 
simulation loop. The PIL simulation also has an interface for a R/C pilot via a USB 
R/C-transmitter-style interface. Aircraft state data can be visualized on the GCS 
software or via FlightGear, a free open-source flight simulator [9J. The PIL simula- 
tion offers the following additional benefits to SIL simulation: 

• Ability to test and identify controller implementation issues before the flight test- 
ing. This helps to determine the limitations on actual hardware and provides im- 
portant information for controller redesign. 

• Provides a real-time testing environment for synthesized controllers. 

• Provides a testbed for integration and testing of hardware and software subcom- 
ponents at the system level. 

• Provides an environment for the pilot and flight test engineers to prepare and 
understand the scope of the flight test and gain confidence in the overall system. 

Beside testing, debugging and validating the control design and implementation, 
the PIL simulation is used for post-flight analysis in validating the simulation model 
that is recursively updated from flight test data. Once the simulation model has been 
sufficiently validated, it can be used to augment and substitute much of the flight 
testing which helps to reduce the risk and developmental costs of the system. 
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6 Flight Testing 

Flight testing is the final stage of testing in the development cycle. Under the current 
concept of operations, a R/C pilot is the pilot in command and has authority over 
what has control of the aircraft (human vs. flight computer). The R/C pilot performs 
the takeoff and landing of the aircraft and transfers control to the flight computer 
once at an appropriate flight condition. A toggle switch on the R/C transmitter is 
used to transfer control of the aircraft to the flight computer via the RxMux fail- 
safe switch. At anytime during testing, the R/C pilot can take over control from the 
flight computer by toggling this switch on the R/C transmitter. Future plans include 
development of autonomous takeoff and landing capabilities, but the R/C pilot will 
remain as a safety backup. 

Flight operations are contained to be within visual range of the R/C pilot and 
under 400 feet AGL altitude. Typical flight durations are 15-20 minutes, depending 
on the purpose of the flight. A normal deployment will last 3-4 hours and have 6-9 
flights. 
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(a) FDI filter output and aircraft states 
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Fig. 7 Experimental flight data analysis 



Flight testing to date has primarily been focused on robust control law and fault 
detection filter research. Figure [6] shows a comparison of SIL simulation data and 
flight data for an //oo control design |7|. This research effort was successful in using 
the entire UAV framework to develop, test, and fly a flight control law experiment. 
This effort also validated the integrated framework design approach and the con- 
cept of using a low-cost UAV platform for flight controls research. An acceptable 
match between the simulation and flight results was achieved despite the low fidelity 
simulation model and the relatively low quality sensors on the UAV platform. 

The model-based fault detection and isolation (FDI) algorithms evaluated using 
the UMN flight test facility were based on robust H^o filter designs. The effect of 
various closed-loop controllers on the FDI filter performance for an aileron fault 
were investigated. Three lateral-directional axis controllers were considered: a clas- 
sical PID design, a LQ optimal multivariable design and a direct model-reference 
adaptive controller (MRAC). The experimental objective were to compare the ro- 
bustness of the three controllers and the performance of the FDI filter to detect the 
aileron fault in the presences of the three controllers. Figures [7^ and|7j3 show the 
flight test tracking performance of the three controllers. RecaU that the controllers 
being investigated are associated with the lateral-directional axes of the aircraft. An 
identical pitch controller is used in all experiments to track 5 deg pitch command 
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reference. The parameter 'AP mocfe ' indicates the status of FCC, i.e. whether the pi- 
lot or onboard flight program (OFP) is in control. The experimental results show the 
varied behavior attained with the robust filter for the different controllers. The ben- 
efit of having a flight test platform is the ability to validate theory and simulations 
on the test aircraft. In 2010, a total of 69 research flights were conducted. 

7 Current Activities and Future Directions 

Currently, the UAV Research Group is pursuing several research appliations for the 
UAV platforms and development framework. 

• The National Science Foundation (NSF) has recently started a program focused 
on Cyber-Physical Systems (CPS) research ifTOll . As part of this research, the 
UAV platforms will play a key role in testing embedded fault detection al- 
gorithms including model based methods, software methods, and data-driven 
anomaly detection methods. These fault detection methods will also be applied 
to real-world industry problems in a collaborative effort with local industry to a 
production UAV platform and air data sensors. 

• Precision landing of small UAVs is a significant challenge given the low quality 
sensors typically used. Many operational losses of UAVs result from the inability 
of the UAV to return reliably to a small protected location. The UAV simula- 
tion and platform will be used to develop precision landing algorithms, sensor 
requirements, and test results in an effort to improve the operational efficiency 
and loss rate of small UAVs. 

• Extensive wind tunnel data is expensive to collect and is rarely openly available. 
Researchers at the NASA Langley Research Center performed significant static 
wind tunnel tests of an Ultra Stick 120 as part of the FASER development ef- 
fort lfm[T2l . This data is being implemented into the UAV simulation and will 
be openly available for research, education, and collaboration. This data set in- 
cludes control surface and thrust effects over an angle of attack range of -5 to 
40 degrees and an angle of sideslip range of +/- 30 degrees. As mentioned previ- 
ously, the UAV Research Group operates an Ultra Stick 120 that was donated by 
NASA LaRC. This aircraft is instrumented with two angle of attack/sideslip vane 
sensors on wingtip booms, which expands the possible research applications of 
this platform to include aerodynamic model identification, gust alleviation, and 
high angle of attack/post-stall flight. 

• The AEM department has developed a "Design, Build, Simulate, Test and Fly 
a UAV" course focused on the use of rapid prototyping software tools for vehi- 
cle modeling, guidance, navigation, and flight control, real-time implementation, 
hardware-in-the-loop simulation and flight tests. The UAV platforms and simu- 
lation framework was used for the class. Students repeated the design process 
of the UAV, including component selection, simulation, flight controller design, 
testing and implementation. At the end of the class, student-designed controllers 
were flight tested on the UAV platform. 
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8 Conclusion 

A low-cost, open source small UAV flight research facility has been developed at the 
University of Minnesota. The UAV Research Group is actively applying the UAV 
platform to flight controls, guidance, navigation, and fault detection research. The 
AEM department is integrating the UAV platforms into the educational curriculum, 
giving students a unique opportunity to work with real flight data and have access 
to a flight test capability. The total cost required to field the UAV aircraft, avionics, 
and sensors described in this paper is under 3,000USD, which is less than the cost 
of most COTS autopilot systems marketed at small UAVs! 
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Adaptive Nonlinear Flight Control and Control 
Allocation for Failure Resilience 
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Abstract. In this publication, reconfiguring control is implemented by making use 
of Adaptive Nonlinear Dynamic Inversion (ANDI) for autopilot control. The adap- 
tivity of the control setup is achieved by making use of a real time identified physical 
model of the damaged aircraft. In failure situations, the damaged aircraft model is 
identified by the so-called two step method in real time and this model is then pro- 
vided to the model-based adaptive NDI routine in a modular structure, which allows 
flight control reconfiguration on-line. Three important modules of this control setup 
are discussed in this publication, namely aerodynamic model identification, adaptive 
nonlinear control, and control allocation. Control allocation is especially important 
when some dynamic distribution of the control commands is needed towards the 
different input channels. After discussing this modular adaptive controller setup, 
reconfiguration test results are shown for damaged aircraft models which indicate 
satisfactory failure handling capabilities of this fault tolerant control setup. 



1 Introduction 

Recent airliner accident statistics (|T||) show that a significant share of these acci- 
dents, namely 17%, is caused by loss of control in flight, where control of an aircraft 
is lost in flight due to a technical malfunction, a piloting mistake or an external up- 
set. This observation motivates flight control research in the field of so-called fault 
tolerant flight control (FTFC). Fault Tolerant Flight Control aims to increase the sur- 
vivability of a failed aircraft by reconfiguring the control laws rather than by means 
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of hardware redundancy only, which is current practice. There are many control 
approaches possible in order to achieve fault tolerant flight control. An important 
aspect of these algorithms is that they should not only be robust, but even adaptive 
in some way, in order to adapt to the faulty situation, see ref. iTTl fTSll . In the category 
of adaptive control algorithms, a distinction is made between indirect adaptive con- 
trol and direct adaptive control. Indirect adaptive control involves two stages. First, 
an estimate of the plant model is generated online. Once the model is available, it 
is used to generate the controller parameters. Instead of estimating a plant model, a 
direct adaptive control algorithm estimates the controller parameters directly in the 
controller. This can be done via two main approaches: output error and input error. 
Of both main categories mentioned here, indirect adaptive control is preferable due 
to its flexibility and its property of being model based. In both categories, there are 
also two sub-versions, namely model reference adaptive control (MRAC) and self- 
tuning control (STC). In the former, one relies on a reference model and works on 
minimizing the tracking error between plant output and reference output (such as 
the concept of sliding mode control). With model reference indirect adaptive con- 
trol it is feasible to achieve three important goals, namely trim value adjustment for 
the inputs, decoupling of inputs and outputs and closed loop tracking of pilot com- 
mands, see ref. |7|. Self-tuning control focuses on adapting the (PID) control gains 
of the controller by making use of the estimated parameter values and is known to be 
more flexible, see ref. ||39l . Currently, much research is performed in the field of in- 
direct adaptive control, where the adaptation is more extensive than only tuning the 
PID control gains. One of these new indirect control possibilities is adaptive model 
predictive control (AMPC), which is an interesting algorithm thanks to its nature to 
deal with (input) inequality constraints. These constraints are a good representation 
for actuator faults. It should be noted that there have already been some successful 
applications of MPC in the field of fault tolerant flight control, see ref. ^ |23l [34ll . 
An alternative indirect adaptive nonlinear control approach is discussed in this pub- 
lication, which allows a reconfigurable control routine placing emphasis on the use 
of physical models to be developed, and thus producing internal parameters which 
are physically interpretable at any time. This technique can deal with control surface 
failures as well as structural damage resulting in aerodynamic changes. 

This publication discusses the combination of the two step method as an iden- 
tification procedure, and nonlinear dynamic inversion as a control method in order 
to obtain a model based fault tolerant flight controller. Another important module 
of this control setup is control allocation. Control allocation is especially impor- 
tant when some dynamic distribution of the control commands is needed towards 
the different input channels. The structure of this publication is as follows. Sec- 
tion[2]provides information on the high fidelity RECOVER simulation model which 
has been used in this research project. The identification procedure is described in 
section [3j while section |4] discusses the nonlinear adaptive control method. Further 
explanations about control allocation can be found in section [5] Simulation results 
are discussed in section 143] for the autopilot. Finally, section |6] presents some con- 
clusions and recommendations for future research. 
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2 RECOVER Simulation Model 

The presented work is part of a research project by the Group for Aeronautical Re- 
search and Technology in Europe (G ARTHUR). This group has estabhshed flight 
mechanics action group FM-AG(16) with the specific goal to investigate the possi- 
bilities of fault tolerant control in aeronautics and to compare the results of different 
reconfiguring control strategies applied to a reference benchmark flight trajectory. 
That benchmark scenario is inspired by the so-called Bijlmermeer disaster of EL AL 
flight 1862, where a Boeing 747-200 Cargo aircraft of Israel's national airline EL 
AL lost two engines immediately after take-off from Amsterdam airport Schiphol in 
the Netherlands and crashed into an apartment building in the neighbourhood while 
trying to return to the airport. A detailed simulation model of this damaged aircraft 
is available from the National Aerospace Laboratory NLR. This RECOVER (RE- 
configurable COntrol for Vehicle Emergency Relief) benchmark model is discussed 
in detail in ref . BOl |4T1 and has been used (also in earlier versions) by a number of 
investigators and organizations ( Il34l[35ll42l ). More information about the reference 
benchmark scenario can be found in ref. 1271 [29ll . Other control strategies and re- 
sults as part of the framework of FM-AG(16) applied to the same benchmark model 
can be found in ref. |l3l[l0l[16l|2Tll22l- Related Fault Detection and Isolation (FDI) 
work can be found in ref. P4l . 

The simulation benchmark for evaluating fault tolerant flight controllers as dis- 
cussed in ref. \4T\ contains six benchmark fault scenarios of varying criticality. 
Fig. [U shows the failure modes and structural damage configuration of the Flight 
1862 accident aircraft, which is the most important fault scenario in the simulation 
benchmark. This publication will focus primarily on the El Al engine separation 
scenario, which is an accurate simulation of flight 1 862, as explained above, which 
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Fig. 1 Failure modes and structural damage configuration of the Flight 1862 accident aircraft, 
suffering right wing engine separation, partial loss of hydraulics and change in aerodynamics, 
source: POI 
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has been validated with data from the digital flight data recorder DFDR and where 
the loss of hydraulics is taken into account. Another scenario is the loss of the verti- 
cal tail, where the vertical tail plane and rudders are detached, resulting in a loss of 
lateral static stability. 

3 Aerodynamic Model Identification 

The identification method considered in this study is the so-called two step method, 
which has been continuously under development at Delft University of Technology 
over the last 20 years, see ref. |8, 24, 36 1. There are many other identification al- 
gorithms mentioned in the literature like maximum likelihood identification (MLI) 
and other one step identification routines, but not all of them are applicable on line. 
One of the few procedures which can be implemented in real time is the so-called 
filtering method developed at DLR, see ref. ||20| . This is a joint state and parameter 
estimation algorithm, but very complex. The advantage of the two step method is 
that it is easier to implement on-line. The key concept of the two step method, is that 
the identification procedure has been split into two consecutive steps, as substanti- 
ated in ref. |9|. More precisely, the global non-linear one step identification method 
is decomposed in two separate steps, where the nonlinear part is isolated in the air- 
craft state estimation step. Consequently, the aerodynamic model parameter identi- 
fication procedure in the second step can be simplified to a procedure which is linear 
in the parameters, but involves nonlinear regressors. The aim is to update an a priori 
aerodynamic model (obtained by means of wind tunnel tests and CFD calculations) 
by means of on-line flight data. The first step is called the Aircraft State Estimation 
(ASE) phase, where the second one is the Aerodynamic Model Identification (AMI) 
step. In the Aircraft State Estimation procedure, an Iterated Extended Kalman Filter 
is used to determine the aircraft states based upon redundant but perturbed infor- 
mation from all sensors (air data, inertial, magnetic and GPS measurements). By 
means of this state information, it is possible to construct the combined aerody- 
namic and thrust forces and moments acting on the aircraft. By means of a recursive 
least squares operation, the aerodynamic derivatives can be deduced. Especially in 
case of structural damage, the aerodynamic properties can change drastically, re- 
sulting in the need for joint aerodynamic model structure selection and parameter 
estimation. This can be done on-line by means of Adaptive Recursive Orthogonal 
Least Squares (AROLS), see ref. [31 1. Validation tests have shown that these meth- 
ods are very accurate. More details about the identification procedure can be found 
in ref. 1 1 3 1 . The identification method is triggered by a monitoring algorithm of the 
innovation (residual). 



4 Adaptive Nonlinear Control 

A major advantage of dynamic inversion is its ability to naturally handle changes of 
operating conditions, which removes the need for gain scheduling, e.g. as is the case 
for classical control methods. This is especially advantageous for control of space 
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re-entry vehicles, due to their extreme and wide operating conditions which vary 
between hypersonic speed during re-entry and subsonic regions during the terminal 
glide approach phase to the runway. Another advantage is its natural property of 
decoupling the control axes, i.e. no coupling effects remain between steering chan- 
nels and the different degrees of freedom. NDI control has been implemented in 
the Lockheed F-35 Lightning II, ( B31 I51). Dynamic inversion is a popular control 
method for flight control and aircraft guidance, (Pl [Tni43l ) as well as reconfiguring 
control, (lU.31i). 

The main assumption in NDI is that the plant dynamics are assumed to be per- 
fectly known and therefore can be cancelled exactly. However, in practice this as- 
sumption is not realistic, not only with respect to system uncertainties but especially 
to unanticipated failures for the purpose of fault tolerant flight control. In order to 
deal with this issue, one can make use of robust control methods as outer loop con- 
trol or neural networks to augment the control signal. However, another solution is 
the use of a real time identification algorithm, which provides updated model infor- 
mation to the dynamic inversion controller. These augmented structures are called 
adaptive nonlinear dynamic inversion (ANDI). The latter procedure is the method 
of choice for this research. 

Figure |2] gives a high-level logical structure overview of the control strategy, 
involving the two step method algorithm and adaptive nonlinear dynamic inversion. 
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Fig. 2 Layout of Adaptive Nonlinear Dynamic Inversion FTFC 



Three consecutive inversion loops have been implemented, namely a body angu- 
lar rate loop, an aerodynamic angle loop and a navigation loop, which can be placed 
in a cascaded order based upon the time scale separation principle. The body angular 
rate loop tracks roll rate p, pitch rate q and yaw rate r by commanding the control 
surfaces aileron 5a, elevator 5e and rudder 5,-- This control law is based upon the 
dynamic equation of an aircraft: ft) — I^'M„ — I^'co x Ift), where ft) is the angular 
rate vector, I is the inertia matrix and Ma are the aerodynamic moments and relies 
on the identified stability and control derivatives for the three angular moments. The 
second loop is the aerodynamic angle loop which tracks roll angle (j) , angle of attack 
a and sideslip angle jS by commanding the body angular rates in the inner loop. The 
navigation loop tracks course x, flight path angle y and speed V by commanding the 
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roll angle (p, angle of attack a and throttle setting T^. The control law consists of a 
kinematics based inversion and a subsequent aerodynamic forces based inversion, 
as elaborated in lfT9l[T8]| . Linear controllers act on each separate NDI loop. They 
involve proportional and proportional-integral control, and gains have been selected 
to ensure favourable flying qualities by means of damping ratio ^ and natural fre- 
quency co„ while complying with the time scale separation principle. Optimization 
of these gain values has been achieved by means of multi-objective parameter syn- 
thesis (MOPS) optimization, see ref. Il26l[32l[33l . The detailed description of the 
control laws for this project can be found in ISOll . 



4.1 Evaluations of Autopilot Experiments on RECOVER: Engine 
Separation Scenario 

The engine separation scenario is a very sensitive situation to combine commands in 
heading, altitude and speed simultaneously. Crucial in this context is to avoid engine 
throttle saturation. Therefore, in this experiment only a heading change has been 
considered, as shown in fig. |3(a)[ Moreover, a limited maximum roll angle of 20° 
has been imposed, due to the restricted safe flight envelope. It has been found that 
altitude and speed changes are also feasible separately, but these are not discussed 
in this publication. 

The time histories of the states in fig. |3(b)| reveal that the aircraft in post fail- 
ure conditions flies with a small nonzero roll angle and sideslip angle, due to the 
asymmetric damage, despite a zero commanded sideslip angle. The control surface 
deflections in figure |4] confirm the cessation of functioning of the control surfaces 
which are powered by the hydraulic circuits connected to engines number 3 and 4, 
as illustrated in fig. [T] The remaining operative surfaces are successful in keeping 
the aircraft in equilibrium and under control, although with restricted authority. 
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(a) tracking quantities 
Fig. 3 Tracking quantities and states for the engine separation scenario 



(b) states (angular rates in (rad/s), velocities 
in (m/s), angles in (rad), distances in (m)) 
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Fig. 4 Deflections of elevators, stabilizer, rudders, ailerons and flaps for the engine separation 
scenario 
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(b) average square innovation as trigger for 
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(a) throttle behaviour 
Fig. 5 Spoilers and specific forces for the engine separation scenario 



Two additional interesting quantities to investigate are the throttle setting and the 
average square innovation A — — X"=i ^ (0' with A =h Ax the innovation (fit- 



ting error), which triggers the re-identification routine as explained in ref. 

Figure [5(a)| confirms that the throttle setting does not saturatqj, however the remain- 
ing control margins in order to remain inside the safe flight envelope are severely 
restricted. This is due to the asymmetric thrust which needs to be compensated by 
the control surfaces. The spike at f = 50s is caused by the feedforward path in the 
controller, which is needed to compensate for the instantaneous speed loss of the 
two lost engines. Figure [5(b)] depicts the values for the average square innovation 
for each force and moment channel separately. At f = 50^, it can be seen that the 
threshold 4,/, = 10 for Ax is exceeded, and a re-identification procedure is triggered 
for Cx- It has become necessary to include the sideslip angle j3, which has become 



More information about throttle saturation in the RECOVER simulation model can be 
found in Iil3il. 



48 



T. Lombaerts et al. 



significant due to the sideslipping flight, as an additional regressor in the identifi- 
cation procedure. This leads to a successful new identification procedure which is 
performed extremely quickly as can be seen in this figure. This result confirms the 
beneficial contribution from the identification routine in this fault tolerant flight con- 
trol setup. The used control allocation strategy is static and equal distribution of the 
control commands over all relevant control surfaces. More advanced techniques are 
considered in section[5] 

5 Control Allocation 

This section describes ways to enhance the performance of Fault-Tolerant Flight 
Control (FTFC) Systems by augmenting them with different Control Allocation 
(CA) methods, based upon a study published in |38|. These methods can be used to 
distribute the desired control forces and moments over the different control effectors 
available, i.e. control surfaces and engines, which makes the control system more 
versatile when dealing with in-flight failures. In a first stage, a number of different 
control allocation methods were compared using a simplified aircraft model. This 
yielded two promising methods: Weighted Least Squares (WLS) and Direct Control 
Allocation (DC A). 

In the WLS method, IflTl . the cost function is written as follows: 



||W„(u-Urf)f + y||W,(Bu-v)r = 



yiW,.B 
W„ 



yzWrV 

W„u^ 



(1) 



Then the optimization problem can be written as: 

uq = argminu||A(u'+p) 
u = u' + p 
u < u < u 



(2) 
(3) 
(4) 



and the residual is computed as: d = b — Au' 

This method is in theory able to find a feasible solution u for all attainable vir- 
tual controls v. This is due to the fact that it is able to free up previously saturated 
actuators. This gives this method much more flexibility compared to other methods. 

Direct Control Allocation (DCA) is an alternative approach to Optimization 
Based Control Allocation. Instead of optimizing some criterion, such as the £2- 
norm, DCA produces a solution based on geometric reasoning. Durham was the 
first one to describe Direct Control Allocation llT2l . Ref. |I6| described the Direct 
Control Allocation problem as: 



maxau* a 

subject to: Bu* = a\ 

u < u* < u 



(5) 
(6) 
(7) 
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Then determine the control u as: 



j;U*ifa> 1 

u* if a < 1 



(8) 



The DCA method looks for the largest virtual control a\ that can be produced in 
the direction of the desired virtual control v. When the maximum virtual control is 
larger than the desired virtual control {a > 1), it scales back the true control input 
to match the two. If the maximum virtual control is smaller than the desired virtual 
control {a < 1), the true control input is not scaled. The benefit of DCA is that it 
will always produce a control effort in the same direction in the control dimension 
space as the desired control effort v. 

These two methods were subsequently implemented in the aforementioned Adap- 
tive Nonlinear Dynamic Inversion (ANDI) controller, discussed in section]?] Using 
the RECOVER model described in section |2l both methods were subjected to the 
engine separation scenario and a few other failures. Both methods were scored on 
a number of metrics and compared with some other methods. For each method, 
the contribution of an Actuator Health Monitoring System (AHMS) was evaluated. 
This system updates the control efficiencies in the mathematical model. The results 
of these tests in fig. |6] reveal that DCA is the best method to be implemented with 
an NDI based FTFC System. Regarding maneuverability, DCA is consistently the 
top performer, especially outperforming the other methods when it comes to turn 
radius, see fig. |6(a)| and Dutch Roll damping, as shown in fig. |6(b)| 
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(a) Turn radius in the engine separation see- (b) Dutch roll damping in the vertical tail loss 
nario scenario 

Fig. 6 Performance comparisons between control allocation variants for the engine separa- 
tion scenario and the vertical tail loss 



For DCA, the addition of AHMS had little impact, although for some other con- 
trol allocation methods there were clear benefits when using this system. The lack of 
effect the AHMS has on the performance of DCA can be explained by the fact that 
this method takes the current actuator position and the rate limits into account when 
computing the solution to the control effort distribution problem, which serves the 
same purpose as AHMS, although in a different way. 
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This study has shown that a significant improvement in the performance of a 
FTFC System can be achieved by including a control allocation method, specifically 
DCA has shown to be very promising in this context as motivated above. The ability 
to use the full potential of the actuator suite helps to keep control over the aircraft 
when a large part of the control power for one of the aircraft axes is lost. More 
details about this study can be found in ref . ||38]| . 



6 Conclusions and Future Work 

Summarizing, it can be stated that, following experiments in a high fidelity simula- 
tion model including validated structural failure modes based upon past accidents, 
the fault tolerant flight control approach based upon the real time physical model 
identification integrated with adaptive nonlinear dynamic inversion is successful in 
recovering damaged aircraft. The designed methods are capable of accommodating 
the damage scenarios which have been investigated in this project. Another impor- 
tant result is that model identification using the two step method has proven to be 
real time implementable in practice. It has been found that adaptive control allo- 
cation is another crucial module for a successful fault tolerant flight controller. An 
extensive comparative study has shown that the Direct Control Allocation method 
is the best method to be combined with an ANDI based FTFC system. The DCA 
method looks for the largest virtual control that can be produced in the direction of 
the desired virtual control. 

It has been found that damage induced flight restrictions are very important dur- 
ing post failure flight. Therefore, future efforts should also be put into the estimation 
of the post- failure safe flight envelope and subsequent optimization of the reference 
trajectory taking into account the new envelope. 
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Coordinated Road Network Search for Multiple 
UAVs Using Dubins Path 

Hyondong Oh, U.S. Shin, A. Tsourdos, B.A. White, and P. Silson 



Abstract. This paper proposes a coordinated road network search algorithm for 
multiple heterogeneous unmanned aerial vehicles (UAVs). The road network search 
problem can be interpreted as the problem to seek minimum-weight postman tours 
with a graphic representation of the road network. Therefore, the conventional Chi- 
nese Postman Problem (CPP) is first presented. We, then, consider physical con- 
straints of UAVs into the search problem, since they cannot be addressed in the 
typical CPP. This modified search problem is formulated as Multi-choice Multi- 
dimensional Knapsack Problem (MMKP), which is to find an optimal solution min- 
imising flight time. The Dubins path planning produces the shortest and flyable 
paths in consideration of physical constraints, so that the Dubins path is used to 
calculate the cost function of the modified search problem. The performance of the 
proposed multiple UAVs road network search algorithm is verified via numerical 
simulation for a given map. 

1 Introduction 

Recently, there have been great efforts to develop cooperative systems of multiple 
UAVs and investigate their benefits. For instance, in military operation, the coopera- 
tive system is able to not only enhance the survivability, but also increases chance to 
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succeed its missions. In terms of practical value, one of the greatest benefits of the 
cooperative system is that it can provide users with better information superiority. 
However, developing such an autonomous cooperative system is quite challenging 
because of technical and operational issues such as decision making and information 
fusion to be solved. 

For reconnaissance, inspection, and intelligence mission, UAVs need to patrol 
some region and gather information. These missions often constraint UAVs path 
and trajectory. For instance, if UAVs are utilized to get some information of enemy's 
activities on the specific roads and military bases, or to observe the traffic of ports 
or roads, they should fly over only those roads or region rather than patrol whole 
terrain. This problem is defined as the road search problem, and also known as 
the vehicle routing problem. The cooperative systems of UAVs can considerably 
improve information superiority in this problem. 

Road search problem has mainly been handled in the operational research area 
lIllEllSllH, and this can be generally classified by two categories: one is Traveling 
Salesman Problem (TSP) which finds a shortest circular trip though a given number 
of cities, and the other Chinese Postman Problem (CPP) finding again the shortest 
path but with path constraints on an entire network of road. The TSP using multiple 
UAVs can be considered as task assignment problem to minimise the cost (time 
or energy) by assigning each target to the UAV and a various methods have been 
developed such as binary linear programming based optimisation |[5j[6l, heuristic 
method including iterative network flow f7l and tabu search algorithm |'8l . On the 
other hand, the CPP is normally used for ground vehicle application such as road 
maintenance, snow disposal ^ and boundary coverage ||10|. 

Vehicle routing algorithms usually approximate their path to a line for reduc- 
ing the computational load, so the physical constraints imposed on the vehicle are 
not addressed. Although some algorithms for the TSP consider the physical con- 
straints, they are mostly developed for a single vehicle fTTl. For multiple vehicles, 
only heuristic method llT2ll is implemented due to the complexity of the problem. 
Moreover, these constraints have been rarely considered in the CPP This paper pro- 
poses a road network search synthesis which considers the physical constraint, as 
well as provides a sub-optimal minimum time solution. We first define the road 
network problem using a graph representation, implement the conventional CPP 
which seeks minimum- weight postman tours of the road network, and then modify 
the search problem in consideration of the constraints of the multiple UAVs. This 
modified search problem is formulated as Multi-choice Multi-dimensional Knap- 
sack Problem (MMKP) ||5] [T3j, which is to find an optimal solution minimising 
flight time. MMKP formulation allows to consider fuel or energy capacity of each 
UAV and different types of roads and vehicles. The Dubins path is used to evaluate 
the performance index of MMKP since it generates the shortest and flyable paths 
between road junctions by using simple geometry fil4< . 

An overview of road network search problem is introduced in Section|2l Section 
[3] presents two heuristic algorithms to solve the conventional road search problem 
for multiple vehicles. In Section|4l this paper addresses the constraints of UAVs and 
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proposes a road search synthesis using the cooperating UAVs. At the last section, 
the performance of the proposed synthesis is verified via the numerical examples. 

2 Road Network Search Problem 

To search the roads identified in the map, a road network should be established, 
which consists of a set of straight line joining waypoints. These waypoints are lo- 
cated either on road junctions or are located along the roads at sufficient separation 
to allow fairly accurate representation of the curved road by a set of straight lines. 
The road network is chosen for this study as shown in Fig. |l(a)| It shows the Google 
map IfTSl of some village in the UK. The road network can be converted to a graph 
as shown in Fig. |l(b)| In order to search all roads within the map, there are two 
typical routing problems Q. 





(a) Google map of village in the (b) Graphic representation 
UK 

Fig. 1 Road network problem 

Travelling Salesman Problem (TSP): A salesman has to visit several cities (or 
road junction). Starting at a certain city, he wants to find a route of minimum length 
which traverses each of the destination cities exactly once and leads him back to his 
starting point. 

Chinese Postman Problem (CPP): A postman has to deliver mail for a network 
of streets. Starting at a given point, e.g., the post office, he tries to find a route of 
minimum length allowing him to traverse each street at least once and leading him 
back to the post office. 

This paper focuses on the CPP and its variation which involves constructing a tour of 
the road network traveling along each road with the shortest distance. Typically, the 
road network is mapped to an undirected graph G = (V, E), and edge weights w : 
E -^ R^ , where the roads are represented by the edge set E and road crossings are 
represented by the node set V . Each edge is weighted with the length of the road or 
the amount of time needed to pass it. The CPP algorithm involves first constructing 
an even graph from the road network graph. This even graph has a set of vertices 
with an even number of edges attached to them. This is required as any traverse of 
the junction by approaching on one road and leaving on another, which means that 
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only an even number of edges will produce an entry and exit pair for the tour. As the 
road network graph may have junctions with an odd number of edges, some roads 
are chosen for duplication in the graph. The technique chooses a set of roads with 
the shortest combined length to minimise duplication. The tour of the even graph 
is calculated by determining the Euler tour of the graph, which visits every edge 
exactly once or twice for duplicated edge. 

3 Heuristic Algorithms for Multi- Agent Road Network Search 

The CPP has a lot of variations such as the Capacitated CPP which capacitates 
the total edge cost they can bear, the Rural CPP which visits certain roads but not 
necessarily all of them, the Windy CPP which has different value for the same edge 
according to the direction and the k-CPP which deals with the deployment of several 
postmen [3|. In this section, Min-Max k-CPP (MM k-CPP) algorithms are described 
for multi-agent road network search with the map as given in Sec.|2l MM k-CPP is 
a variation of k-CPP which considers the route of the similar length. This objective 
can be required if UAV should finish road search mission with the minimum mission 
completion time. The MM k-CPP problem was first mentioned by Ii 1 6il and later, 
solved by several algorithms ^. This study implements two heuristic algorithms 
for the MM k-CPP with the similar way as in the literature and compared the results. 

3.1 Cluster Algorithm 

This algorithm is based on the cluster first - route second approach. In other words, 
in the first step, the edge set E is divided into k clusters and then, a tour for each 
cluster is computed. This algorithm can be represented as a constructive heuristic 
method, and described as follows l|2l . 

Algorithm description 

1. Determine the set F of representative edges 

First of all, k representative edges fi, . . . , fk of cluster Fi for each vehicle are 
determined. Let /i be the edge having the maximum distance from the depot 
and /2 be the edge having maximum distance from /i . The rest of representative 
edges are successively determined by maximizing the minimum distance to the 
already existing representatives. Then, remaining edges are assigned to the clus- 
ter according to the weighted distance between e and fi considering the distance 
between representative edges and depot, number of assigned edge to the cluster 
Fi and cost of the cluster. 

2. Include edges for connectivity 

Add edges between every vertex and depot and determine minimum spanning 
tree 111 which includes original edges in each cluster for connection between the 
edges. 
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3. The rural Chinese Postman Problem 

Compute CPP route of required subset of edges out of total edges by using the 
conventional Chinese Postman algorithm explained in ifTTl . 

Figure |2] shows the result of Cluster Algorithm which is applied to the scenario 
given in Sec. 2 with four ground vehicles. In this figure, solid line represents the 
road visited once and dashed line represents duplicated path for Even graph added 
by the CPP algorithm. Total distance of all vehicle is 24340.7 metre and maximum 
distance of one vehicle is 7318.3 metre for agent 1. 




(a) Agent 1 



Jmm. 











(b) Agent 2 





(c) Agent 3 



(d) Agent 4 



Fig. 2 Search path result of the cluster algorithm. Solid line is vehicle path visited once and 
dashed line is duplicated vehicle path. 



3.2 First Route Algorithm 

Unlike the cluster algorithm, the First Route algorithm follows a route first - cluster 
second approach. In a first step, postman tour which covers all edges is computed, 
and then, this tour is divided by k tour segments which have the similar length. This 
algorithm is described as follows. 



Algorithm description 

1 . Compute an optimal 1 -postman tour C* using the conventional Chinese Postman 
algorithm 

2. Compute splitting nodes 

k — 1 splitting nodes {vp^ , ■ ■ ■ , Vp^_-^) on C* are determined in such a way that 
they mark tour segments of C* approximately having the same length. Approx- 
imated tour segment length, Lj is computed by using Shortest Path Tour Lower 

Bound, Sjnax- 
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Smax = t; niax {w{SP{vi,u)) +w{e) +w{SP{v,vi))} (1) 

Z e={u,v}eE 
L3 - {^-^){W{C* - 2s„a.)) + S™ax, 1 < J < fc - 1 (2) 

where k denotes the number of vehicles, w{a) represents the distance of the 
subtour a and SP represents the shortest path between nodes considering road 
network. Then, the splitting node v^. is determined as being the last node such 
that w(C* ) < Lj. C*^ is the subtour of C* starting at the depot node and 
ending at w„. The details can be found in ITSll . 
3. fc-postmen Tours 

Constructed k tours C = {Ci, . . . , Cfe} by connecting tour segments with short- 
est paths to the depot node. 

Figure |3] shows the result of the First Route algorithm which is applied to the sce- 
nario given in Sec. 2 with four ground vehicles. Total distance of all vehicle is 
24340.7 metre and maximum distance of one vehicle is 7818.5 metre for agent 4, 
which is slightly longer than the Cluster algorithm. Although both of two heuristic 
algorithms are intuitive, easy to implement and show reasonable performance with- 
out heavy computation burden for multiple ground vehicles, they can be far from an 
optimal solution and difficult to consider characteristic of the vehicle. 



4 Coordinated Road Network Search for Multiple UAVs 

For the road network search using multiple UAVs, variation of the typical CPP algo- 
rithm is required, so that it can consider the operational and physical characteristic 
of the UAV in the search problem. As shown in Fig.|4l since the UAV cannot change 
its heading angle instantaneously due to the physical constraint, the trajectory has 
to meet the speed and turn limits of the UAVs. Moreover, differently from ground 
vehicle, the UAV does not have to fly along the road only to cover a certain edge 
which is not connected. This modified search problem is formulated as MMKP 
which is to find an optimal solution minimizing flight time. Classical MMKP is 
to pick up items for knapsacks for maximum total value so that the total resource 
required does not exceed the resource constraints of knapsack fT3\. For applying 
MMKP to the road network search, UAVs are assumed as the knapsacks, the roads 
to be searched are resources and limited flight time or energy of each UAV is capac- 
ity of knapsack. MMKP formulation allows to consider the limitation of each UAV 
flight time and different types of roads, vehicles and minimum turning radius and 
get the sub-optimal solution of the coordinated road search assignment. Moreover, 
the Dubins path planning produces the shortest and flyable paths taking into con- 
sideration their dynamical constraints, thus the Dubins path is used to calculate the 
cost function of the modified search problem. The details of proposed road network 
search algorithm for multiple UAVs are as follows. 
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Fig. 3 Search path result of cluster algorithm. Solid line is vehicle path visited once and 
dashed line is duplicated vehicle path. 

4.1 Generation of the Shortest Edge Permutation 

First, unordered feasible edge permutation is generated with a given petal size. Petal 
size means the maximum number of edge that can be visited by one UAV and it is 
determined by available resources of each UAV. In case that the end vertex of one 
edge and any vertex of next edge are not connected, connect them with the edge 
which has shorter distance. Then, shortest order-of-visit edge permutation among 
the permutations which consist the same edges is computed under the assumption 
that the path is line. 



4.2 Dubins Path Planning 

Once shortest edge permutations are determined, next step is to compute and store 
the cost (path length or flight time) of them. After UAVs complete their mission. 



Fig. 4 Road network Search 
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(a) The 2D Dubins arc geometry 
Fig. 5 Geometry of Dubins path planning 



(b) Determination of intermedi- 
ate pass angle 



since they should come back to the depot, line path from the end of edge permuta- 
tion to the depot is connected additionally. In this step, this study uses the Dubins 
path which takes into account the orientation and path constraints of the UAV in- 
stead of using Euclidean distance of each edge. The Dubins path is the shortest path 
connecting two configuration under the constraints of a bound on curvature. It is 
formed either by concatenation of two circular arcs with their common tangent or 
by three consecutive tangential circular arcs. Using the principles of differential ge- 
ometry, the Dubins path can be determined by curvature in two dimensions ifTSl . For 
the 2D manoeuvre, the initial and final tangent vectors are coplanar and the straight 
line manoeuvre is not uniquely defined for this case and it must be calculated. The 
Dubins arc is shown in Fig. |5(a)| The vector sum for the position vector p which is 
a position of the final point pf relative to the start point Ps in start axes is given by: 



P^Pf P^ 



P. = r., a. 



a 



f 



"/ 



(3) 



This equation can be arranged for the vector c which connects the centres of the 
turn circles as: 

c = etc = P - Ts + T/ = -as + ac + af (4) 

where tc and c are the centre vector and its length, respectively. The connecting 
vectors a^, aj and Oc can be written in terms of the start basis vectors. Then, the 
centre vector equation, Eqn. Q now becomes: 



(5) 



c^ ctc = R{9s) ±1 _ ±1 



where a is the distance of vector Oc, Og denotes the rotation angle of the first turn, 
R(0s) represents direction cosine matrix, and Kg and Kg are the maximum curvature 
of initial and final turn, respectively. If both initial and final angle are given, a set 
of four paths are produced and the shortest path is selected. In this paper, the pass 
angle for the Dubins path is determined by simple geometry. First of all, consider 
the edge vectors defined by the three vertices Wi_i, v, and w^+i, and let path unit 
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vector Wi and w,+i and unit vector along the bisector of the angle formed by the 
three vertices w as in Fig. |5(b)| Then, the intermediate pass angle /3 which makes 
the path remain near the roads is given by Eqn. Q. Initial and final angle of each 
permutation are assumed to be zero. 

P^tan-\^)±^ (6) 

4.3 MMKP Formulation and MILP Optimisation 

Final step of the proposed algorithm is to allocate edge permutation to each UAV 
to cover every edge with the minimum total time. This can be expressed MMKP 
formulation and given by: 

min J = 2. /, "^j^ij 

i=i j=i 

NuAV ^Pi 

subject to ^ ^EkjXij >1, k e {1,..., N^dge} (7) 

J=l j=l 

^Pi 

^ Xij = 1, i G {1, . . . , NuAv} 

i=i 

x„ e{0,l}, i<^{l,...,NuAv}, je {!,..., A^pJ 

where Njjav^ N^dge, and Np. denote the number of UAVs, edges to be visited 
and permutaions generated by the i-th UAV, respectively. Tj represents mission cost 
(flight time) of j-th permutation and Ekj represents the matrix whose fc-th element 
of j-th permutation is 1 if edge k visited, otherwise and xij is either 0, implying 
permutation j of the i-th vehicle is not picked, or 1 implying permutation j of the i- 
th UAV is picked. First constraint represents that UAVs should visit every edge once 
or more and second one represents the allocation of the exact one edge permutation 
to the each UAV. This MMKP problem is solved by SYMPHONY MILP (Mixed 
Integer Linear Programming) solver 1191 . 



5 Numerical Simulations 

To evaluate the performance of the proposed search algorithm, numerical simulation 
is performed with four UAVs and the map given in Sec. 2. UAVs have the following 
characteristic: 

• Minimum turning radius pmm- [50 40 30 30] metre 

• Maximum cruise speed Vc.max'- [40 35 25 25] m/s 

Maximum curvature Kmax of UAVs for the Dubins path can be computed by 
K-max = ^/Pmin- UAVs are assumcd to have maximum cruising speed during entire 
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(a) UAV 1 



(b) UAV 2 




(c) UAV 3 
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(d) UAV 4 
Fig. 6 Multiple UAVs road search result 




(e) Total UAV paths 



simulation and maximum petal size of edge permutation is set to 4. Figure |6] shows 
the road network search result. It can be observed that the longest flight path is given 
UAV 1 whose speed is the fastest among four UAVs and flight path is smooth and 
flyable due to the Dubins path planning. Moreover, every roads are searched once 
and UAVs come back to the depot as shown in Fig. |6(e)| As explained in the pre- 
vious section, since the UAV does not need to fly along the road only to cover all 
edges for the minimum time, the road search result includes additional paths which 
connect some of edges or the edge and depot. 

6 Conclusion 

This paper described a coordinated road network search synthesis for multiple 
UAVs. This paper first defined a road network search problem as a graph and dealt 
with it using the conventional CPP algorithm. For the application to multiple UAVs, 
the search algorithm was modified in the consideration of the physical constraint of 
UAVs. The modified problem was solved by the proposed synthesis which mainly 
consists of the Dubins path planning and MMKP formulation for the shortest and 
flyable path with the minimum time solution. The performance of the proposed syn- 
thesis was verified via numerical simulation for the given map. 
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Eigenstructure Assignment and Robustness 
Improvement Using a Gradient-Based Method 

Erik Karlsson, Stephan Myschik, and Florian Holzapfel 



Abstract. This paper presents a gradient-based method for increasing the robustness 
of multivariable systems, measured by the stability margins of the broken SISO 
loops. The method uses iterative perturbations of the closed loop eigenvalues to 
minimize a certain cost function containing weighted gain and phase margins and 
the variation of the eigenvalues. The initial and perturbed closed loop dynamics are 
specified using eigenstructure assignment. The algorithm presented is used in the 
gain design process of the lateral part of the flight control system for a generic trans- 
port aircraft. Results obtained with this novel approach are analyzed regarding the 
SISO and MIMO stability margins. 



1 Introduction 

The eigenstructure assignment method for assigning the dynamics of multivariable 
systems has been widely used in the aerospace sector, especially in the design of 
aircraft flight control systems ([7], [11], [12]). The method offers great freedom in 
the eigenvalue placement via state or output feedback. Extra degrees of freedom 
provided by independent control inputs can be utilized to specify parts of the cor- 
responding eigenvectors. However, with the exact assignment of eigenvalues and 
eigenvector components, the method features no optimization of the robustness of 
the closed loop system compared to other multivariable design techniques such as 
//^loop-shaping ([10]). As the requirements on the closed loop eigenvalues 
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are rarely defined as absolute values, a slight variation is usually allowed. This 
provides room for increasing the robustness using numerical algorithms. 

Several approaches to a more robust eigenstructure assignment have been 
presented, e.g. multi-objective optimization via genetic algorithms ([5], [6]) or 
eigenvector projection methods ([9]). 

The well-known gain and phase margins for single-input/single-output (SISO) 
systems have since long been used as a measure of robustness for such systems. In 
general, they cannot be applied straight forward on the different loops of multiple- 
input/multiple-output (MIMO) systems. This is due to the fact that the SISO mar- 
gins only consider uncertainties present in one loop. Uncertainties may be present 
in multiple loops, and have cross-coupling effects not considered by the SISO 
margins. The SISO margins have however been extended to multivariable systems 
using the minimum singular value of the return difference ([2], [3]) and further on 
using structured singular value analysis ([10], [13]). In flight control applications, 
stability margin requirements are however given as gain and phase margin re- 
quirements on the SISO loops. 

This paper presents a first order gradient-based algorithm aiming at optimizing 
the SISO stability margins. This is done by an iterative perturbation of the closed 
loop eigenvalues in a direction, which minimizes a cost function containing the 
SISO margins and the eigenvalue perturbations. The MIMO margins are however 
also examined in order to ensure robust stability for the entire system. 

The paper consists of the following parts. First, an approach to the eigenstruc- 
ture assignment for a general multivariable system is given, with a pseudo-inverse 
based projection to find the best achievable eigenvectors ([4]). The MIMO stabil- 
ity margins are presented using the structured singular value of the sensitivity 
function matrix. Then, a gradient-based algorithm for improving the closed loop 
stability margins via eigenvalue perturbations is presented. Thereafter, the pre- 
sented gradient-based algorithm is used in the gain design of the lateral-directional 
part of a flight control system for a generic transport aircraft. The results are ana- 
lyzed regarding the SISO and MIMO stability margins. In the last part, conclu- 
sions are drawn from the results. 



2 Eigenstructure Assignment 

The eigenstructure assignment procedure described in this section follows the one 
given in ([4]), other approaches can be found in ([6], [9], [11], [12]). 

Consider a linear, time-invariant, multivariable system on state space form 

x(f) = Ax(/) + Bu(r), (I) 

with system matrix A 6 R""" , input matrix Bg R""^ , state vector xg R" and in- 
put vector UG R"\ The system has m independent control inputs. Assuming all 
states are not available for feedback, output feedback is used. The output equation 
is given by 
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y{t) = cx{t), (2) 

with output matrix C e R''^" and output vector yeR^ The control law 

u(f) = -Ky(r) + Uc(?), (3) 

with the constant feedback matrix Ke 7?"^'^ and commanded input u^(f) leads to 
the closed loop dynamics 

x(f) = (a -BKC)x(?) + BUc(f) = Ax(r) + Buc(/) . (4) 

Purpose of the control design is now to determine the feedback gain matrix K in 
such a way that the closed loop system matrix A = (A - BKC) is given the de- 
sired dynamics. The closed loop eigenvalues A- and corresponding right eigenvec- 
tors V, are determined by the equation (5). 



(A-BKC)-v, =v, a, , i = l,...,n 



(5) 



Equation (5) can be rewritten on matrix form to include the input directions 
z. = KCv, . This leads to equation (6). 



[AJ-A b]- 



KCv, 



[AJ-A b]- 



0. 



(6) 



The desired eigenvalues of the closed loop system must not be the same as those 
of the open loop system, i.e. the matrix [/l-I - A bJ must have full rank (be in- 
vertible). All nontrivial solutions v, , z. to the equation system (6) must be within 
the null space (the kernel) of the matrix [/l-I - A bJ . This means that any possi- 
ble solution can be described as a linear combination of the base vectors n, of the 
null space. The null space base vectors can be divided into one upper part n^ re- 
lated to the eigenvectors, and a lower part n. related to the input vectors. 



N, 



■k 



N 



^ 



(7) 



Now, the achievable eigenvectors can be written as 



h.Ai 



!n,Zi 



■■^Arh 






(8) 



where 1^ is an arbitrary m-dimensional parameter vector. Sorting matrices 
(P* and iP'' I V, are introduced to sort the elements of the eigenvectors in 
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those specified (number of specified elements is s) and those unspecified (num- 
ber of unspecified elements is hence n-s). Elements of greater importance are 
specified, whereas lesser important elements remain unspecified. This leads to 
equation (9). 



" (vfL, 



\ ' /.sxn 
1^1- L-s)x. 



(9) 



Through the use of a weighting matrix Q^ the elements of the desired eigenvec- 
tor can be assigned greater or lower importance, giving the weighted desired 
eigenvectors 



Q. 



(10) 



and the relation between the desired eigenvectors vf, and the parameter vector 
1 , , described by equation (11). 



(Qj„./kL=(Q.,L-(Ni.L-(i,i 



(11) 



Now two different cases are possible. If the number of elements to be specified is 
equal to the number of independent control effectors available, i.e. s = m, an exact 
assignment of the specified eigenvector elements is possible. In this case the pa- 
rameter vector is given by 



1a,=[QvNIJ"'-Qvv 



(12) 



If the number of elements to be specified is larger than the number of control ef- 
fectors available, i.e. .y > m, no exact assignment is possible. By using a (Moore- 
Penrose) pseudo-inverse based projection onto the null space, the parameter vector 
can be calculated according to 



i.=[Kr-QvNil'-(Nir-Qvv 



^f,- 



(13) 



With the best achievable parameter vector known (either through exact assignment 
or approximated through projection on the null space), the achievable eigenvector 
and the corresponding input directions are given by 



z, =N, -1. 



v,=N, -1, 



This leads to the desired gain matrix, shown in equation (15). 

K = [z, z, - zJ-(C-[v, V, - vj)-'. 



(14) 



(15) 



The resulting gain matrix now places the closed loop eigenvalues as desired, and 
yields the specified eigenvector elements or their best approximation. 
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3 Robustness Enhancement Using a Gradient-Based Method 



The method described in this section aims at increasing the gain and phase mar- 
gins of the broken SISO loops of the closed loop system. The robustness of such a 
system can either be measured by the mentioned stability margins of each broken 
loop, assuming uncertainties only occur in one loop at a time. Or, more general, 
using the conservative multivariable stability margins, which consider concurrent 
uncertainties in multiple loops ([1]). 

The multivariable stability margins are in the following given using the struc- 
tured singular value of the sensitivity function matrix. 

Consider a generalized multiple-input/multiple-output system with plant G 
and constant feedback matrix K , extended by an inverse multiplicative input un- 
certainty matrix A = diag {A,,...,A,jJ containing uncertain elements A^ = r^. e'* , 
k = \,...,m (uncertain complex numbers located within a circle of radius r^. [10]), 
see Fig. 1. The transfer function from the output w of the uncertainty block to the 
input z is then given by 



z = [i + K-G]"'-w = S 



w . 



(16) 



where S is the sensitivity function at the actuator inputs. The system can now be 
represented by a structure containing only S and A , connected via w and z . 
Assuming S is stable, robust stability for such a system is guaranteed (for all al- 
lowed perturbations) if and only if the Nyquist plot of det[l - SA( j(y)J does not 
encircle the origin, i.e. when det[l-SA(j(y)J;^ 0, V<jj,VA ([10]). The structured 
singular value //(s) of the sensitivity function is defined as the inverse of the 
smallest gain c which destabilizes the system (// = l/c), i.e. for which 
det[l - cSA( j(y)J = . This definition of robust stability of the SA -system can now 
be written as 



^l{s{jco))■a{^{jo)))<\,y(D. 



(17) 




Fig. 1 Multivariable feedback system with uncertainties. 
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This means that robust stability is guaranteed when the product of the structured 
singular value of the sensitivity function and the maximum singular value of the 
uncertainty matrix is less than 1 for all frequencies (a "generalized small gain 
theorem") ([10]). Since S{jco) is a frequency dependent function matrix, robust 
stability is ensured only when the maximum singular value of the uncertainty ma- 
trix is smaller than the minimum value of the inverted structured singular value of 
the sensitivity function over all frequencies. This leads to the relation (18). 

With the definition of the minimum value of the inverted structured singular value 
of the sensitivity function matrix, k^^ , the multivariable gain and phase margins 
are given in ([3]). They are defined in equation (19). 

GM = — ^- — , PM = +6' with 6' = 2-arcsin|^ . (19) 

The different loops of the system may be disturbed by a set of gains A satisfying 
the condition 1 /(l + k^^ ) < A < 1 /(l - k^^ ) , or by phases d satisfying the condi- 
tion 1^1 < 2arcsin(^^j, /I) , without leading to an unstable closed loop system. 

The aim is now to perturb the closed loop eigenvalues, given by the eigenstruc- 
ture assignment, in order to increase the gain and phase margins of the broken 
loops of the system. The loops are cut both at the actuator inputs and at the sensor 
outputs. When one loop is cut open, the others remain closed. Assuming the 
number of actuator inputs is q and the number of sensor outputs is r , the task can 

be formulated as a constrained optimization problem with the cost function to be 
minimized given by 

e{p) = -- vL (p)Wc„ Vc„ (p)-- v™ (p)W™ v™ (p) + 

^ ^ (20) 

+-ip-PdY^p{p-Pd) 

with vectors ^gm'^pm ^ ^* '^ containing the cut loop gain and phase margins. 
The parameter vector p e /?' contains those of the closed loop eigenvalues al- 
lowed to be varied (or some parameter given by the eigenvalues, e.g. relative 
damping or natural frequency, time constants etc.). The vector p^ g R' consists of 
the corresponding desired values. Weighting matrices ^gm '"^pm ^ R^i^''>^^i^''> 
consider the gain and phase margins and W e 7?'^' weights the eigenvalue 

variations. All weighting matrices are diagonal and positive definite. The weight- 
ing matrices can either be identity matrices, or chosen to emphasize the improve- 
ment of certain gain or phase margins. One approach is to specify the weighting 
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coefficients according to the relative distance of the corresponding margin from a 
certain required value. The smaller the margin, the more weighted it is. Thus, the 
algorithm focuses on improving the worst margins. 

The constraint under which the cost function is to be minimized is that the 
eigenvalues remain within the boundaries defined by certain maximum and 
minimum values, p^^, ^ P ^ Pmax • The cost function is iteratively minimized 
using a variation of the eigenvalue parameter vector given by the steepest descent 
direction. 



Ap = -T] 



dEip) 
dp 



(21) 



with the step length /; > . The step length selection is a tradeoff between a suffi- 
cient reduction of the cost function and the computation time of finding the appro- 
priate step length. The selection method chosen is based on the Armijo sufficient 
decrease condition ([8]), ensuring that the step length is not chosen too small. The 
step size is iteratively increased from an initial value until an appropriate step 
length is found, fulfilling the condition 



£(p - j]VE) < E{p) - cTjVE'^VE , 



(22) 



for a constant c e [0,lj . A maximum step length is also defined in order to limit the 
selection computation time. When performing a gain design over thousands of 
grid points, the number of iterations in each point greatly influences the total com- 
putation time. The gradient of the cost function is given by equation (23). 



dp 



[w„ 



dp 



[W, 



av. 



ap 

+ W,(p-pJ. 
The Jacobian matrices of the gain and phase margin vectors, 



(23) 



av. 



ap 



3pi 



dp, 



dpi 



av. 



ap 



3''l.PM 
dpi 

dpi 



M,PM 
dp, 

dp, 



(24) 



have to be determined through numerical differentiation, e.g. using Newton's dif- 
ference quotient, given by 



av, _v,(p+4>;)-i',(p) 



a^j 



^j 



, i = \,...,q + r , j = 1,...,/ . 



(25) 



Here dp is a vector with a small step dp : in the j:th parameter. When the pa- 
rameter vector is varied by Ap , the value of the cost function is decreasing 
according to 



74 E. Karlsson, S. Myschik, and F. Holzapfel 

'dE~ 



AE- 



8p 



\dEy 


"dE' 

L^pJ 



Ap = -ri- ^— ^^ <0. (26) 



If the calculated variation Ap leads to a parameter vector not satisfying the con- 
straints, the elements exceeding are adjusted and set to the closest bound. 

This algorithm leads to an improvement of the included stability margins under 
the given constraints on the eigenvalue variation. 

4 Implementation and Results 

The eigenstructure assignment method is implemented in the gain design of the 
lateral control system for a generic transport aircraft, described in detail in [4]. 
The model dynamics is linearized for horizontal, straight-forward flight at an alti- 
tude of 6000 m and a Mach number of 0.4. The aircraft configuration features a 
total mass of 1 10 tons. Flaps and gear are retracted. 

The inherent dynamics has roll and spiral mode time constants T^ = 0.99 s and 
Tj = 27.4 s. The Dutch roll dynamics is given by the relative damping 
^£1^ =0.171 and natural frequency cOg^jn =0.633 rad/s. 

A control allocation scheme converts the lateral control effectors aileron, rud- 
der and roll spoilers into two independent virtual controls for the creation of roll- 
ing and yawing moments, u = [^^o;;,^ki„J , giving the possibility to decouple the 
roll and yaw axis. 

The initial roll mode time constant, T^ _, = 1 .4 s, ensures level 1 handling quali- 
ties. With this choice, level 1 handling qualities for the given aircraft can still be 
achieved while obeying maximum control surface deflections in the linear gain 
design process. An inherently stable spiral mode ( T^ ^ = 1 .0 s) is chosen to pro- 
vide automatic wings leveling for small bank angles and bank angle resistance to 
disturbances. A Pl-command filter, not described in this paper, provides neutral 
spiral stability from the pilot's point of view ([4]). The desired Dutch roll dynam- 
ics is given by the relative damping ^^^ ^ = V 2 / 2 and a natural frequency 

^o.oK.rf = 2.0 rad/s. 

The linearized lateral model contains the stability axis roll and yaw rate, the 
sideslip and roll angle, the integrator states for the control error integration as well 
as the position and translational states of the actuators. The output vector used for 

feedback contains all but the actuator states, y = [r^,/^,p^,^,^i,/?if . The two 

independent control variables make it possible to exactly assign two elements of 
each eigenvector. 

Roll rate and angle are set to zero in the Dutch roll eigenvector, yaw rate and 
angle of sideslip to zero in the roll and spiral mode eigenvectors. 

After the initial eigenstructure assignment, the eigenvalues are perturbed 

iteratively and reassigned, with the parameter vector p = [7'j,7'^,^, 



'q.dr J 
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Table 1 Stability margins before and after robustness enhancement. 
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Fig. 2 Gain margins for the broken SISO loops, measured at the actuator inputs and sensor 
outputs. 
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Fig. 3 Phase margins for the broken SISO loops, measured at the actuator inputs and sensor 
outputs. 
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Fig. 4 Curves for the parameter vector elements. 

containing the roll and spiral mode time constants and the Dutch roll dynamics. 
The elements of the parameter vector are bounded, constraining the adjusted 
closed loop dynamics to remain within specified limits, in this case Tg <1.4s, 
r^ < 1.8 s, ^o^ < 0.8 and ft;^^^ > 1.6 . 

The SISO stability margins before and after the iterations are presented in Ta- 
ble 1 together with the MIMO margins. All gain and phase margins have been im- 
proved by the algorithm. Especially large improvements can be seen in the phase 
margins of the yaw control input and the roll rate and angle of sideslip sensor out- 
puts. Fig. 2 shows how the gain margins at the actuator inputs and sensor outputs 
are improved during the iteration. Fig. 3 depicts the corresponding phase margins. 
It can be observed how the roll command and the roll rate and angle sensor gain 
margins show similar curves during the iteration. When compared to the changing 
closed loop dynamics described in Fig. 4, the roll gain margins are depending on 
the roll and spiral mode time constants. The roll command and roll rate phase 
margins have the same dependence in Fig. 3. Gain margins of yaw command and 
yaw rate sensor are at first decreasing as the Dutch roll relative damping is in- 
creasing. When the damping reaches its maximum allowed value, the yaw margins 
start to increase again, and keeps increasing until the Dutch roll natural frequency 
reaches its minimum value. 



5 Conclusions 



The presented gradient-based algorithm offers a simple and fast way of improving 
the stability margins of a multivariable system via iterative perturbations of the 
closed loop eigenvalues. It has been successfully implemented in combination with 
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an eigenstructure assignment algorithm and is used in the gain design process of a 
flight control system for the lateral motion of a generic transport aircraft. The algo- 
rithm has been shown to converge to the best eigenvalues, minimizing the cost func- 
tion under the given constraints. The stability margins of the SISO loops as well as 
the multivariable stability margins are increased. By examining how the stability 
margins vary with the parameters of the closed loop dynamics, relations between 
specific margins and time constants or damping/frequencies can be identified. 

The focus of the presented method has been the applicability to real control de- 
sign problems. The method has been proven useful on a typical problem in flight 
control design: vary the closed loop dynamics within limits in order to achieve 
improved gain and phase margins. 
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Helical Flight Path Trajectories for Autopilot 
Evaluation 

Geitjan Looye 



Abstract. A helical flight path trajectory (helix) involves flying exact circles over 
the ground while climbing or descending at a given flight path angle and speed 
profile. The manoeuvre is challenging to fly in windy conditions, since the path 
reference is inertial whereas the aircraft naturally tends to move with the air mass. 
Tracking a helix introduces periodical lateral and longitudinal wind shears in turn. 
This makes the helix an excellent manoeuvre for testing autopilot control laws, al- 
lowing to evaluate co-ordination of longitudinal and lateral modes, tracking accu- 
racy along a curved flight path, combined tracking of inertial (flight path) and air 
mass-based references (airspeed), and to evaluate the trade-off between behaviour 
in turbulence and wind shear. Since helical flight path trajectories are not a standard 
option in most autopilot / flight management systems, this chapter derives generally 
applicable reference variables and high-level control strategies for use with typical 
autopilot structures. This allows the reader to fly the helix manoeuvre using his or 
her own autopilot design. As an example, simulation and flight test results for an 
autopilot developed for DLR's test aircraft ATTAS will be discussed. 
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1 Introduction 

A major challenge in the design of autopilot flight control laws is to achieve sat- 
isfactory closed-loop aircraft performance during severe atmospheric disturbances, 
like wind shears, turbulence and wake vortices. Especially in the case of passenger 
aircraft, besides tracking accuracy of velocity and flight path, careful attention has to 
be paid to aspects such as ride comfort, control (throttle!) activity, and safety limits. 

During several projects the DLR Institute of Robotics and Mechatronics was in- 
volved in, helical flight path trajectories were found to be extremely useful to assess 
design aspects listed above. During a helix manoeuvre, one basically encounters a 
considerable amount of challenges in autopilot design: co-ordination of longitudinal 
and lateral modes, accurate tracking of a flight path that is continuously "bending 
away", and combined tracking of inertial (the flight path) and air mass-based refer- 
ences (the airspeed). From an atmospheric point of view, during a helix manoeuvre 
even little constant wind results in continuous wind shears in lateral and longitudinal 
directions. 

The idea of performing the helix test manoeuvre came from publications by 
Kaminer and Lambregts. Kaminer flew small UAVs along helical flight paths for 
observation purposes f\\, also proposing the trajectory as a benchmark. Lambregts 
demonstrates the ability to accurately fly circular ground tracks in windy conditions 
using a Control Wheel Steering mode developed in the NASA TCV program Q. 

This chapter will focus on the helix manoeuvre as a benchmark trajectory for 
autopilot control laws. Using flight mechanics principles, generally applicable ref- 
erence variables and high-level control strategies for autopilot control laws are de- 
rived, allowing the reader to make his or her autopilot to fly this manoeuvre for 
testing purposes. As an example, the results from simulations and a recent flight 
test using DLR's ATTAS aircraft will be discussed, highlighting interesting design 
aspects that are typically revealed by flying the proposed benchmark trajectory. 



2 The Helix Trajectory 

This section geometrically describes the helix trajectory and discusses relevant flight 
mechanical aspects. The trajectory as used in this work is depicted in Fig.[T] It starts 
at the point Init and ends at the point Final. The over-all manoeuvre begins with a 
straight segment. This allows for easy capture by the aircraft using standard func- 
tionality in the autopilot. Geometrically, the actual helix is initiated at the point 
where the initial straight segment perpendicularly passes the helix centre. After the 
helical segment, the trajectory ends with a straight segment, providing a clear and 
safe point to stop the manoeuvre by making sure the aircraft is in trimmed, wings 
level flight. Handling the discontinuous transitions between the straight path seg- 
ments and the actual helix will be discussed in Section [33] 

The trajectory is defined by the parameters in Table [1] The objective for an au- 
topilot is to capture the initial straight segment of the helix manoeuvre, track the 
inertial helix flight path at a commanded Vcas (which may be varied during the task. 
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Fig. 1 Definition of the helix trajectory 
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Fig. 2 Top view of helical trajectory 
(straight segments have been omitted) 



Table 1 Parameters describing the helix trajectory in Fig.[T] 



Helical segment: 



I Initial and final straight segments: 



''center GPS position of the helix center 

Rhelix Helix radius 

VcAS,,rf„- Calibrated airspeed 

hend Altitude at which helix ends 

n,um Number of full turns 

dir Direction (counter-) clockwise 



^CAS,„i, VcASfi,„,i Calibrated airspeed 

hi„i, - Initial altitude 

yinii y final Inertial flight path angle 

Xinii Xfinal Inertial track angle 



of course), and return to steady symmetric flight, ending at the point marked Final. 
The challenge is to make sure the ground track is a single circle, even in case of 
steady wind. The control strategy to achieve this can be derived from basic flight 
mechanics principles, as will be discussed in the following. 

Fig. 12] shows the true airspeed vector va (magnitude Va), wind vector vw (mag- 
nitude Vw) and the inertial speed vector v (magnitude V, necessarily tangent to the 
circle). The variables VVand Xw are given wind speed and direction respectively. 
Furthermore, the variables Va and x are the momentary airspeed and track angle on 
the reference trajectory respectively. It is of interest to compute V and the crab angle 
Xcriib — Xa~ X (Xa as defined in Fig. O that are necessary to stay on the circular 
path. Assuming that cosJa ~ 1 , cos^w ~ 1 , cosy w 1 (with Ya the air mass-referenced 
flight path angle), it is easy to derive the following two equations: 



V ^Vw cos{xw - X) 



Xcrab = - arc Sin 



V^^V^sin'iXw-X) 

V,sm{xw-X y 
Va 



(1) 
(2) 
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Fig. 3 Inertial speed and crab angle as a Fig. 4 Bank angle required to maintain helix 
function of the track angle for fixed airspeed radius {Va = 160 kts, Rheiix = 2000 m, y = 

deg) 

The inertial speed and crab angle thus vary as a function of the angular position of 
the aircraft on the helix path. This is illustrated in Fig. [3j where the inertial speed 
and crab angle have been plotted as a function of the track angle during a full turn. 
The numbered wind conditions correspond with Fig. [21 

The term V'n,sin(;^iy — ;t) in equation [2] is the lateral wind component w.r.t. the 
inertial flight path (positive to the right). Its time derivative is the lateral wind shear 
that has to be compensated for: 



-Vwcos{xw-x)X, with: x 



Vcosy 



R. 



(3) 



helix 



The autopilot thus has to make sure the crab angle is continuously adapted in order 
to stay on the circular track. This is usually already achieved by feedback of x 
and sufficient weather cock stability, e.g. provided by a good yaw damper. More 
important is the effect on the bank angle /i required to maintain a circular flight 
path. This angle is easily derived from lateral equilibrium in a co-ordinated turn |[3l: 



mV^ cos^ 7 



R 



he It 



mg tan [J. so that: tan/i 



V'- 



gR, 



heli: 



7 y 

■ cos" y = —X cos 7 (4) 



where g is the acceleration of gravity. Since V varies as a function of the track angle, 
the bank angle jx must vary as well in order to keep the horizontal distance to the 
centre of the helix constant. An example for 7 = is depicted in Fig.|4l 

At this point, it is interesting to make a short side step to two more scenarios. 

Constant airspeed / bank angle turn 

Maintaining a constant bank angle based on a constant airspeed results in turning 

relative to the moving air mass: 



tan/i 



V 



gR. 



helb 



■ cos^ 7a 



(5) 
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Fig. 5 Comparison between constant and ground- Fig. 6 Aircraft deviation from 
speed corrected bank angle (wind and flight param- circulai' flight path 
eters as inFig.|4} 

This equation (usually with ]^ = 0) is incorrectly used in some text books. The 
constant bank angle causes a superposition of the moving air mass due to wind and 
the turning aircraft, causing it to circle away from the helix centre. Fig. |5] compares 
the trajectories resulting from continuously adapting and constant bank angles. 

Constant ground speed turn 

Returning to eqn.|4l it is obvious that in case a constant bank angle is desired while 
staying on track, this can only be achieved by maintaining a constant ground speed. 
This is not common practice, but it was for example used by the autopilot of the 
Lockheed SR-71 Blackbird, since reconnaissance sensors required (near) constant 
bank angle turns for focusing on their specific targets on the ground Q . 

For a more general discussion on flight mechanical aspects of manoeuvring under 
significant wind conditions, the reader is also referred to the work of Rysdyk |i5|. 



3 Automatic Helix Tracking 

Obviously, the helix trajectory is not a standard mode in autopilot and flight man- 
agement systems (FMS). In order to to allow the manoeuvre to be flown by a given 
autopilot, appropriate command signals have to be provided. These signals will be 
derived from the flight mechanical considerations discussed above. 

A typical autopilot control law structure as used in most commercial transport air- 
craft is depicted in Fig.|71 Only relevant connections have been labelled. Most struc- 
tures consist of inner loops for stability and command augmentation (SCA), middle 
loops for flight path and speed tracking, and guidance loops for lateral (LNAV) and 
vertical navigation (VNAV). The signal processing block prepares feedback signals 
for use by the autopilot loops, including computation or estimation of variables not 
measured directly, smoothing airdata signals by complementary filtering with ap- 
propriate inertial measurements, etc. 
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Fig. 7 Typical autopilot / FMS control law structure 

The VNAV and LNAV functions are usually provided by the FMS; the flight 
path and speed tracking modes may also be directly controlled by the pilot via the 
Flight Control Unit (FCU). Most Flight Management Systems use the available au- 
topilot altitude and climb modes for vertical navigation by commanding appropriate 
altitude (he) and flight path angle (/c) or rate of climb commands (he). Lateral navi- 
gation is usually done using an internal LNAV control law that generates roll angle 
commands {(pc) for the autopilot SCA loops. Alternatively, the FMS may provide 
lateral path error (ye), turn rate (Xc) and track angle (Xc) commands to an autopilot 
LNAV mode. 

3.1 Lateral Control Strategy 

Fig. |6] sketches the a top view of the situation in which the aircraft has drifted away 
from the reference circular flight path. The intention is to maintain constant cali- 
brated airspeed (Vcas)^ without imposing time constraints on the navigation along 
the helix path. Therefore, as a reference for computing position errors and track an- 
gle commands, always the momentary radial line is used. This line is computed in 
local geodetic co-ordinates from the aircraft and helix centre GPS positions, using 
algebra and parameter values that come with the WGS-84 standard ^. From the 
radial line, the currently desired track angle Xc can be easily determined and pro- 
vided to the autopilot. The radius error Rg — R^c — Rheiix is provided as the lateral 
path error (ye, see Fig.|7]). The commanded turn rate is Vground/ Rheiix (eqn.O, where 
V^roimd is the momentary ground speed (Vground = Vcosy). The nominal bank angle 
command is to be computed from eqn.|4](not eqn.|5|). Most autopilot SCA systems 
track roll angle (^c) rather than bank angle (jXe) command signals, since measured 
is directly available from most Inertial Reference Systems (IRS£|. For transport 
aircraft usually jx k, <p, since pitch attitude angles (Q) are relatively small in most 
flight phases. This in turn allows ^e ~ Mc • 



3.2 Longitudinal Control Strategy 

The actual helical flight path starts at an altitude hj„j,i,eiix and ends at he„d- The 
reference altitude depends on the angular position and the number of the turn (see 
Fig.IB: 



For an explanation of the fundamental difference between bank and roll angles, see e.g. IS). 
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{2n{i,urn - 1 ) + Zf - Xinit )Rhelix + hi,utj,elix (6) 



where it„r„ is the number of the current turn and hi„i, j^eiix is the altitude at which the 
helix starts. The first term is the tangents of the desired flight path angle: 



tan Yc 



\ Inriturn^helix 



(7) 



The command signals he and jc may be directly provided to the autopilot control 
laws (Fig.|7]). 

3.3 Transition to the Helix Trajectory 

The complete trajectory as depicted in Fig.[T]contains two important transitions: (1) 
from the initial straight segment onto the actual helix and (2) from the helix onto the 
final straight segment. These transitions should be well timed and smooth in order to 
prevent large path deviations and flight crew discomfort. For vertical path tracking, 
most autopilots provide such functionality by early switching to altitude and flight 
path angle commands from the next segment (see for example 1,3J). For lateral path 
tracking, a similar technique is used. The functionality is briefly described, since it 
is not provided by most autopilots. 

Fig. [H (left) shows a top view of the aircraft about to capture the helix. Even 
though the aircraft is still on the straight segment, the geometric angular and po- 
sition errors w.rt. the helix may already be computed. Obviously, the track angle 
error would make an autopilot turn the aircraft to the left {jlc. track < 0). The radial 
error would make it turn right {j^c. radial > 0). The reference bank angle, computed 
from eqn.m is to the right (jJ.c,ref > 0). The best point to capture the helix is where: 
I li-c, track I = I Mere/ + l^c.radiai I ; initially resulting in zero bank angle command. With 
the aircraft approaching the helix, jlc,track will usually decrease more rapidly than 
l^c.radiai SO that the aircraft will slowly bank to the right and nicely capture the cir- 
cular flight path. Leaving the helix and capturing the final straight segment can be 
done using the very same principles. 
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4 Flight Test and Results 

The helix manoeuvre was performed three times during a flight test using DLR's 
fly-by-wire test bed ATTAS (Advanced Technologies Testing Aircraft System) on 
June 10, 2009. ATTAS is a highly modified VFW-614 small passenger aircraft with 
two turbofan engines Q). The purpose of the flight test was to evaluate autopilot 
control laws developed at the DLR Institute of Robotics and Mechatronics regard- 
ing tracking accuracy of flight path and calibrated airspeed, with particular attention 
to aspects like piloting^ mode switching, comfort, and control activity during at- 
mospheric disturbances. The autopilot control laws are partly described in |8:|; the 
SCA and lateral path tracking functions are based on Nonlinear Dynamic Inversion 
(NDI), vertical path and speed tracking is based on the Total Energy Control System 
(TECS, 13). 

The autopilot command signals were computed as described in the previous sec- 
tion and has been implemented in a dedicated software routine. The autopilot LNAV 
mode was used. The helix manoeuvre was integrated in landing approaches to run- 
way 26 of Braunschweig airport by setting the parameters in Table [T| appropriately, 
see Fig.Qfl. The manoeuvre is initiated in an altitude of 7500 ft (hi„it) in the direc- 
tion of the runway, with Yinit — Odeg. The helix centre {rhgn^^) is located at a distance 
Dheiix = 3200 m before the runway threshold. The latter point is also the reference 
for computing local x,y,z co-ordinates. After exactly three full turns the manoeu- 
vre ends with the final straight trajectory, which matches the standard ILS approach 
path. Shortly before ending the final circle, the ILS signal is checked. If the aircraft 
is within the cone, the autopilot changes into its ILS tracking mode. Otherwise, 
flight continues in runway direction, but at constant altitude hf,„d- The calibrated 
airspeed is held constant at 160 kts (selected by the pilot on the FCU). During the 
final quarter of the last turn, it is reduced to the adopted final approach speed of 140 
kts. All approaches were followed by a go-around; automatic landing capability had 
already been successfully flight tested before [7J. 

The trajectories as resulting from the flight test are depicted in Fig. (TO] The he- 
lical approaches are hardly distinguishable and the ground tracks are nearly perfect 
circles. This is very encouraging, since the weather conditions were quite challeng- 
ing for achieving accurate tracking of the helical flight path. At the airport winds 
between 15 and 20 kts from direction 255 deg (runway heading 265 deg) were mea- 
sured. At higher altitudes winds up to 28 kts were encountered, see Fig.[TT] Between 
and below clouds (broken cumulus, base 4000 ft, top 7000 ft) thermal turbulence 
was encountered. In the clouds, turbulence levels were considerably higher. 

Since the wind conditions were strongest during the first approach, the discussion 
in the following will focus on this part of the flight test. The path tracking accuracy 
is confirmed by Fig. [121 Here the time along the helix path has been stretched along 
the horizontal-axis while the lateral deviation is represented by the vertical axis. 



^ This allowed for the organisation of an add-on noise measurement campaign on the 
ground, evaluating helical approaches as an alternative potential noise abatement proce- 
dure, see IIOV For this reason, also standard Instrument Landing System (ILS) based and 
steep landing approaches were flown to provide noise reference values. 
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Fig. 9 The helix trajectory 
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Fig. 10 Trajectories plotted from flight data 
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Fig. 11 Estimated wind speeds during 
first helical approach 



Most of the time the lateral path deviation is well within the wing span of the 
aircraft. Two peaks occur: when capturing the helix (left) and when capturing the 
ILS path (right). They arise at the moment of switching to the helix path and to 
the ILS modes respectively. The error results from the early activation for smooth 
capture as discussed in Section [331 and nicely demonstrates disturbance rejection 
performance. 

The next prime tracking variable is the calibrated airspeed. A first challenging 
aspect of the helix trajectory is the artificial creation of wind shears relative to the 
flight path. This can clearly be seen from Fig. [13] showing the calibrated airspeed 
and inertial speed for the first helical approach. Clearly, during the helix the autopi- 
lot manages to track the calibrated airspeed reference quite well. As expected, the 
ground speed varies considerably (see also Fig. 3). This situation is typical for wind 
shear: while holding the airspeed approximately constant, the ground speed gets a 
strong gradient. 

As briefly addressed in Section[2l it is common practice to smooth air data signals 
using complementary filtering. For acceleration feedback, the following signal is 
formed: 



^compl 



XS+\ 



^CAS- 



XS 



ts+l 



-V 



(8) 
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where the time constant T determines the complementary frequency content of both 
measurements. During the helix, the ground speed varies periodically, causing peri- 
odical variations in its derivatives as well. This causes continuously nonzero gradi- 
ents of the acceleration signal, in turn causing Vcompi to continuously deviate from 
VcAS- This explains the periodical error in the calibrated airspeed signal in Fig.[T3l 





Fig. 12 Lateral path deviation during first Fig. 13 Calibrated airspeed, ground speed 
helix approach (the distance between two and complementary speed 
solid horizontal lines indicates the aircraft 
wing span) 

The bank angle during the first helix approach is depicted in Fig. [14] As expected, 
it varies strongly as a function of time (compare with Fig.|4ll and accurately follows 
the values commanded by the autopilot. The side slip angle is quite noisy and not 
directly available to the control laws. It therefore is estimated instead |[8l. The result- 
ing smoothed signal (black) remains within 1 deg during the manoeuvre, indicating 
excellent weathercock stability and turn co-ordination. 

The crab angle is depicted in Fig. [15] During the initial straight segment (to 
the left) a crab angle of two degrees is already necessary due to a small wind 



Roll angle 
Side slip angle (; 
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Fig. 14 Bank, side slip, and estimated side Fig. 15 Crab angle 
slip angles 
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component from the left. Fig. [16] depicts the altitude profile, comparing measured 
and commanded values. The initial error arises at the moment the helix is initiated 
and is quickly reduced. The error remains within a 5 m band most of the time. 
Larger peaks occur between 100s and 300s, where relatively strong turbulence was 
encountered (now shown). The slight undulation is caused by the varying ground 
speed. 





Fig. 16 Commanded and actual barometric Fig. 17 Mean fan shaft speed A'l of both en- 
altitudes gines 

Finally, the mean fan shaft speed of both engines is depicted in Fig.[17l Providing 
good tracking performance in considerable turbulence, thrust control activity by the 
autopilot was low, very much pleasing the flight crew. This important feature is 
attributed to the Total Energy Control System. 



5 Summary 

Helix manoeuvres are interesting trajectories for evaluating and validating autopi- 
lot flight control law performance. The main objectives of this chapter have been 
to discuss relevant flight mechanical aspects and to derive appropriate command 
signals that allow such a manoeuvre to be flown (in simulation or flight test) by 
a given autopilot. As an example, flight test results of an example autopilot have 
been discussed, addressing helix-related performance challenges such as path and 
speed tracking accuracy, influence of wind shears, engine throttle activity, and path 
segment capture behaviour. Detailed metrics for these performance aspects are un- 
der development and will allow for quantitative comparison of different autopilot 
designs. 
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Maneuver Envelope Determination through 
Reachability Analysis 



E.R. van Oort, Q.P. Chu, and J.A. Mulder 



Abstract. Knowledge of the safe maneuvering envelope is of vital importance to 
prevent loss of control aircraft accidents. In this paper, determination of the safe 
maneuvering envelope is addressed in a reachability framework. The forwards and 
backwards reachable sets for a set of initial trim conditions are obtained by solving 
a Hamilton Jacobi partial differential equation through a semi-Lagriangian method. 
Results obtained using this approach are presented for a nonlinear, high-fidelity, 
F-16 aircraft model. 



1 Introduction 

During the last decades adaptive control in its many forms has received a lot of atten- 
tion within the flight control community. These control algorithms are able to deal 
with changes in the system's dynamics due to possible system component faults 
and failures. A question that still remains unanswered is which parts of the state 
space are safe to operate, often even when the dynamics of the system are com- 
pletely understood or assumed known. This question is of fundamental importance 
in the safety verification of control systems and system validation. Recent statistics 
show that the majority of accidents in aviation nowadays are due to Loss-of-Control 
(LOC) im |2l . This also shows that LOC is not a phenomenon solely attributed to 
military aircraft, but is equally a real problem for commercial aircraft and general 
aviation. 

The relevance of knowledge of the flight envelope is emphasized by means of 
an accident which ultimately is the result of a violation of the safe flight enve- 
lope. On October 4, 1992, a Boeing 747 cargo plane crashed into two apartment 
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buildings in the Bijlmermeer neighborhood of Amsterdam, near Schiphol Airport. 
Engine number three separated from the right wing of the aircraft shortly after take- 
off, damaging the wing flaps, and struck engine number four which also separated. 
Analysis showed that the aircraft still had marginal controllability left in a severely 
restricted flight envelope |3J. Simulator experiments using various different fault 
tolerant flight control approaches have shown that landing the aircraft safely was 
possible ||4l|5]|6|- Additional simulations showed that a very experienced pilot was 
able to land the aircraft using the standard control system when he was informed 
about the severely restricted flight envelope. However, the Boeing 747 aircraft does 
not have such a fault tolerant control system and, more importantly, the pilots did 
not have any knowledge about the restricted flight envelope of the aircraft. When 
the crew tried to reduce the speed for landing the aircraft banked sharply to the 
right without chance of recovery: control of the aircraft was completely lost with 
disastrous results. 

In this paper a semi-Lagrangian level set approach is taken to obtain the safe 
maneuvering envelope for aircraft dynamics that are assumed known. The safe ma- 
neuvering set is defined as the intersection between the forwards and backwards 
reachable sets for a given set of a-priori known safe states. This approach has been 
used to solve fluid flow problems, for example in Q and fSj. The novelty in this 
work is the application of the semi-Lagrangian approach to systems with control 
and disturbance inputs, and its application in higher dimensions on kd-tree grids. 
The method is applied to a high-fidelity nonlinear F-16 model at different flight 
conditions and configurations to demonstrate their effects on maneuverability. The 
safe maneuvering sets obtained can be used for trajectory generation, path planning 
and controller synthesis, even in post-failure conditions such that safety is improved. 

The paper is organized as follows. First, in section[2]the concept of safe maneuver 
envelope is defined, and how it can be obtained through reachability analysis. Then, 
in section [3] the level set method and the Semi-Lagrangian approach are discussed. 
The longitudinal maneuver envelope for an high-fidelity model of an F-16 aircraft 
is evaluated in section|4]at different fiight conditions. Finally, section|5]draws con- 
clusions from the work presented, and states future research directions. 



2 Maneuver Envelope and Reachability 

The conventional definition of the flight envelope is "[the flight envelope] describes 
the area of altitude and airspeed where an airplane is constrained to operate." ^. 
The flight envelope boundaries are defined by various limitations on the perfor- 
mance of the airplane, for example available engine power, stalling and buffet char- 
acteristics, structural considerations and requirements on maximum noise produc- 
tion. A common way to present the flight envelope is the doghouse diagram which 
relates the altitude, velocity and possibly other variables at which the aircraft can 
safely fly. 
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2.1 Flight Envelope Protection 

As noted in | IQl one of the most promising techniques to prevent LOC-related ac- 
cidents is envelope protection. The technique tries to prevent the aircraft from ex- 
ceeding the safe maneuvering envelope. A prerequisite for flight-envelope protec- 
tion systems is a fly-by-wire (FBW) system. In the FBW, the pilot's inputs are sent 
to a computer which then calculates the desired commands, i.e. there is no direct link 
between the pilot and the controls. Such systems have existed for over 30 years but 
are currently only used in military aircraft, several commercial aircraft, and a very 
limited amount of general aviation aircraft. A prerequisite for protection is accu- 
rate knowledge of the maneuvering envelope, both to safeguard against excursions 
and LOC events, and not place too conservative constraints on performance of the 
aircraft. Furthermore, when the envelope is known, the maneuvering space can be 
presented to the pilot to increase situation awareness. 

2.2 Safe Maneuvering Envelope 

The boundaries defined on the flight envelope in the doghouse plot are adequate dur- 
ing normal operation of aircraft. The main problem with the conventional definition 
of flight envelope is that only constraints on quasi- stationary aircraft states are taken 
into account, for example during coordinated turns and cruise flight. Additionally, 
constraints posed on the aircraft state by the environment are not part of the conven- 
tional definition. The aircraft's dynamic behavior can pose additional constraints 
on the flight envelope, for example due to inertia coupling effects. Such constraints 
would especially be important for military and acrobatic aircraft, aircraft having ex- 
perienced upset, and aircraft with airframe and/or actuator damage or malfunctions. 
Thus, an extended definition of the flight envelope is required which will be called 
the safe maneuver envelope. 

Definition 1 (Safe Maneuver Envelope). The safe maneuver envelope is the part 
of the state space for which safe operation of the aircraft and its cargo can be guar- 
anteed and external constraints will not be violated. 

The safe maneuver envelope is defined by the intersection of three envelopes: 

• Dynamic Envelope: Constraints posed on the envelope by the dynamic behavior 
of the aircraft, due to its aerodynamics and kinematics. 

• Structural and Comfort Envelope: Constraints posed by the airframe, pilot, pas- 
sengers and cargo. These constraints are defined through maximum accelerations 
and loads. 

• Environmental Envelope: Constraints due to the environment in which the air- 
craft operates. 

The last two envelopes pose external constraints on the flight envelope, constraints 
which are generally well-known and can be quantified easily. Examples of such 
external constraints are the terrain and no-fly zones around the aircraft, and the 
maximum load-factor the airframe can sustain before breaking. The example given 
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in the introduction is considered to be a violation of the dynamic flight envelope. 
The focus in this paper is on the first type, i.e. the flight envelope directly related 
to the aircraft's dynamic behavior. A more formal definition of the dynamic flight 
envelope is given below. 

Definition 2 (Dynamic Fliglit Envelope). The region of the aircraft's state space in 
which the aircraft can be safely controlled and no loss-of-control events can occur. 

Constraints posed on the aircraft by the dynamic flight envelope are for example 
a maximum roll-rate at high angle of attack in order to prevent the aircraft from 
entering a potentially hazardous inertia coupling phenomenon or spin. 

2.3 Reachable Sets 

Reachable set analysis is an extremely useful tool in safety verification of systems. 
The reachable set describes the set that can be reached from a given initial set within 
a certain amount of time, or the set of states that can reach a given target set with a 
certain time. The dynamics of the system can be evolved backwards and forwards in 
time resulting in the backwards and forwards reachable sets respectively. The differ- 
ence between these two sets is illustrated in figure [T] For a forwards reachable set, 
the initial conditions are specified and the set of all states that can be reached along 
trajectories that start in the initial set are determined. For the backwards reachable 
sets, a set of target states is defined, and a set of states from which trajectories start 
that can reach that target set are determined. 

Assume that the dynamics of the system are given by 

X = f{x,u,d) (1) 

where x G R" is the state of the system, u & 'W C R™ is the control input, and 
fif £ ^ C M'' a disturbance input. Then, the formal definition of the backwards and 
forwards reachable sets is given by definitions[3]and|4]respectively. 

Definition 3 (Backwards Reaciiable Set). The backwards reachable set ^{t) at 
time t(0 < T < //), of the system O]) starting from the target set ,%, is the set of 
all states x(t), such that there exists a control input u{t) £ ^(t < / < f/), for all 
disturbance inputs d{t) £ &{x <t< tf), for which some x{tf) G ,% are reachable 
fromx(T) along a trajectory satisfying ([T]l. 

Definition 4 (Forwards Reaciiable Set). The forwards reachable set Y{x) at time 
t(0 < T < f/) of the system ([T]l starting from the initial set 5^q, is the set of all states 
x{t), such that there exists a control input u{t) G "^{t <t< tf), for all disturbance 
inputs d{t) G ^(t <t<tf), for which x{t) is reachable from some x(0) G S^o along 
a trajectory satisfying ^. 
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(a) Backwards Reachable Set (b) Forwards Reachable Set 

Fig. 1 Backwards and forwards reachable set definitions. 

2.4 Safe Maneuver Envelope through Reachable Set Analysis 

Now, the safe envelope for a given system with a prehminary set of safe states can 
be found by the intersection of the forwards and backwards reachable set of this 
safe set. States that are part of both these sets can be reached from the safe set, 
and can reach the safe set within a certain time. Therefore, if the initial/target set is 
known to be safe, then all states that are part of both the forwards and backwards 
reachable sets can be considered safe as well. This is illustrated by figure [J] For 
example, an aircraft can enter a spin starting from a certain initial condition, and the 
spin trajectory would then be included in the forwards reachable set. If recovery to 
a safe flight condition from the spin is possible, the spin trajectory, or part of it, is 
also included in the backwards reachable set. A similar example is deep stall of the 
aircraft. 



backwards 
reachable set 



safe 
operating set 



forwards 
reachable set 




Fig. 2 The safe envelope for a known safe set is defined by the intersection of the forwards 
and backwards reachable sets. 
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3 Semi-lagrangian Reachable Set Analysis 

Sets can be represented either explicitly by enumerating all parts belonging to the 
set, or implicitly as the level set of some function. This function can be evolved in 
time such that the reachable set can be tracked through time by tracking the level 
set of the function in time. In this section different solution methods to track the 
interface evolution methods over time are discussed. 



3.1 Time Evolution 

Explicit and implicit surface descriptions only give a representation of a set. To 
obtain the reachable set the interface has to be evolved in time. Suppose that the 
velocity of each point on the interface is given by an external velocity field f{x,t). 
The simplest method to move the interface with the velocity field is by solving the 
ordinary differential equation 

f=/(.V) (2) 

for every point x on the interface, a Lagrangian formulation of the interface evo- 
lution equation. To avoid the problems with instabilities, and deformation of the 
interface elements, an implicit function (p is used both to represent the interface and 
to evolve it. A simple convection partial differential equation 

(p, + V(p-/(.r,0=0, (3) 

where the subscript t denotes a temporal partial derivative in the time variable f , and 
V is the gradient operator. This is an Eulerian formulation of the interface evolu- 
tion, since the interface is captured by the implicit function (p as opposed to being 
tracked by interface elements as was done in the Lagrangian formulation ifTTlfTSll . 
This equation can be solved using an upwind schemes combined with forward Euler 
integration. Stability of this approximation has to be enforced using the Courant- 
Friedrichs-Lewy (CFL) condition IT3l . stating that numerical waves should propa- 
gate at least as fast as the physical waves. This leads to the CFL time-step restriction 
of 

At±\ii^<a (4) 

where < a < 1 is a safety factor usually taken to be 0.9. This condition clearly 
poses a stringent constraint on the allowed time-step for high resolution grids in 
combination with fast dynamical systems. 

The time restriction posed by the CFL condition can be eliminated by allowing 
unbounded stencils lfT4l . The time-step can be decoupled from the CFL-condition 
by using an explicit, unconditionally stable time-stepping scheme. These schemes 
can be interpreted as semi-Lagrangian schemes. The CFL condition is satisfied for 
large time-steps by shifting the stencil. The first order hyperbolic partial differential 
equation Q propagates the values (p along the characteristic curves s{t) defined by 
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--f{s.t) 



(5) 



Thus, the value of cp at any time t can be determined by finding the characteristic 
curve passing through (x, t) and tracing it backwards in time to a point (xq, /q) where 
the value of (p is known, then (p{x,t) — (p(jfo,^)- This observation is the basis of the 
backwards characteristic, or the Courant-Isaacson-Rees (CIR) scheme ifTSl . the sim- 
plest semi-Lagrangian scheme. Given cp at time t^, the CIR-scheme approximates 
the (p (x, t^^ ' ) at any point x at time f '^^ ' ~ t'^ + At hy evaluating the velocity /(x, t'' ) 
and approximating the backwards characteristic through x by a straight line 



^Atf{x/)^s{t'). 



(6) 



Then, (p(x,r*^+' ) is set equal to the interpolated value at the location s{t^). For linear 
PDEs, the Lax-Richtmyer equivalence theorem guarantees that CIR converges to 
the exact solution when At, Ax — > if the discretization is stable and consistent llT4ll . 
Semi-Lagrangian schemes combine the regular mesh of an Eulerian scheme with 
the unconditional stability of a Lagrangian scheme. The difference between the Eu- 
lerian, Lagrangian and semi-Lagrangian schemes is shown in figure [S] 




Euler Lagrangian Semi-Lagrangian 

Fig. 3 Euler, Lagrangian, and semi-Lagrangian schemes for the Level Set equation. 



3.2 Hamilton- Jacobi Partial Differential Equation 

For systems with control and disturbance inputs the level set equation ^ needs 
to be reformulated as an Hamilton-Jacobi partial differential equation. One input, 
marked as input b, will try to keep the system away from the target or to initial set, 
the other, input a, will try to drive it towards the target or away from initial set. 
The reachable set can then be obtained as the viscosity solution of a time dependent 
Hamilton- Jacobi-Isaacs equation ifT^llTTll . 

The dynamic programming approach yields the backwards reachable set as the 
viscosity solution of the terminal value problem, with subscript b indicating back- 
wards. 



dV_ 
dt 



-mm 



0,//fc(x,^) 
ax 



:0, y(x,o) = r(x) 



(7) 
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where the Hamiltonian is defined as 



Hb{x, 3-) = minmax 3- f{x,u,d). (8) 

ox II d \ ax J 

Similarly, the forwards reachable set can be found as the viscosity solution of the 
initial value problem, with subscript / indicating forwards, 



dV 

^r — hmax 
dt 



0,H,ix,'^) 



-0,V{x,0)^S{x) (9) 



dV^ . fdV^^ 



where 

///(x, ^) =maxmin ( 4-^ 1 f(x,u,d). (10) 

ax u d \ax J 

T{x) and S{x) are functions for which the level describes the target and initial sets 
,"7 and .^ respectively. The comparison with in (|71l and ^ is added such that the 
reachable set is only allowed to grow over time lfT6ll . 

4 F-16 Longitudinal Maneuver Envelope 

In this section the longitudinal maneuver envelope for a high-fidelity nonlinear 
model of an F-16 aircraft is determined at two different flight conditions. 

4.1 F-16 Model 

When the full maneuvering capabilities of an aircraft are considered the reachable 
set calculations have to be run in 8 dimensional space, if the altitude is considered 
to be fixed. Three states related to the airspeed, three rotational rates, and two states 
defining the relevant attitude. In this example only the longitudinal dynamics are 
considered to reduce the computational load, and simplify representation of the ma- 
neuver set. Therefore, only four states are considered: the airspeed Vj, the angle 
of attack a, the pitch rate qs, and the pitch attitude defined through a quaternion 
component 172- The engine thrust and stabilizer deflections are considered as control 
inputs. In this particular case, no disturbance inputs were considered. However, it 
is possible to include uncertainty on the aerodynamic parameters, aircraft param- 
eters, and wind as disturbance inputs and obtain the worst-case safe maneuvering 
envelope. 

The dynamics for the F-16 model are given by 

^^ = T^ {-D{a,qB,dh) + T cos, a + mgi) 
a = qB + j;^ {-L{a,qB,8h) -T sma + mgi) 
qs = ^M{a,qB,5h) 
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where 



gi 
83 



-qh 



2go^2 sin a + (^0 - ^2 ) ^os a] g, 



^l-q\)^ima\g, 



with (70 a quaternion component, and g the gravitational acceleration. The horizontal 
stabilizer deflection 5/, is constrained to ±25 degrees, and the engine thrust T is lim- 
ited between and 75000 Newton. The lift, drag and pitching moment are computed 
through lookup-tables obtained from ifTSl . In order to calculate the optimal control 
inputs to evolve the level set, a linear approximation in the stabilizer deflection of 
the drag, lift and pitching moment coefficients was made as, for example 



C,„j, (a, 5/,) « Cm^{a) + C,„ (a)5/, 



(11) 



The true coefficients were used to propagate the implicit function over time. An- 
other option is to pose additional constraints on the elevator deflection such that 
at its maximally allowed deflection the minimal/maximal aerodynamic effects are 
generated. The leading-edge flap is not deflected in any of the simulations. 

4.2 Scenarios and Trim Set Determination 

Before the reachable set calculations can be performed, first the safe set has to be 
defined. In this case the aircraft was trimmed for straight and level flight at Om and 
10000m altitude for a center of gravity location of 0.30% of the mean aerodynamic 
chord. The result is a trim curve relating the airspeed and the angle of attack for 
each of the flight conditions and configurations described above. Figure [4(a)| shows 
the trim curves for each of the considered scenarios. 




a [deg] 



100 150 

Vt [m/s] 

(a) 



200 -lO 



(b) 



Fig. 4 Trim curve of the F- 16 aircraft model at low and high altitude (a). Extended trim curve 
(b). 
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The trim curves have to be translated into implicit surfaces before the reachable 
set calculation can be performed. This can be done by calculating the weighted dis- 
tance to the nearest trim point for each point in the domain of interest. Implicit 
surface methods require that at least one grid node is within the initial or target set 
such that the location of the interface can be found. Therefore, the converted trim 
curve is not sufficient to initialize the level set calculations. Hence, a region within 
a certain weighted distance of the trim curve is considered to be safe, where the 
weighted distance defining the band should be larger than the smallest allowed grid 
cell. A different option would be to propagate a narrow band on a high resolution 
grid over a small timestep, and then start the calculations with the resulting reach- 
able sets as initial set on a coarser grid. Another option would be to extend the trim 
set to include non-stationary, non-level flight conditions and create an implicit set 
description from that set. This would however require a very large number of trim 
points. 

In order to limit the computational load and clearly show that the forwards and 
reachable set yield different reached portions of the state space, the evolution time 
is set to 1 second. This time was reached by evolving the initial/target set forwards 
or backwards in time respectively, by 100 steps of 0.01 seconds. The computation 
time for the F-16 application was between 1 and 4 hours on a single core of an Intel 
Xeon X5500 processor running at 2. 13 Ghz and equipped with 12 GB of RAM. 

4.3 Airspeed Comparison 

First of all a comparison is made between the maneuverability of the F-16 aircraft 
at low altitude for different airspeeds. Figures [5(a)[ and |5(b)| show the reachable sets 
and safe envelope at airspeed of 60 and 150 m/s respectively. Clearly, with increas- 
ing dynamic pressure, the aircraft becomes more maneuverable as can be observed 
from the increased size of the safe maneuver set. Furthermore, the expected rela- 
tions between the angle of attack and the pitch attitude, the angle of attack and pitch 
rate, and the pitch attitude and pitch rate can all be observed from the plots. 

4.4 Altitude Comparison 

There exists a large difference between the trim curves of the aircraft at low and 
high altitude as can be observed from figure |4(a)| The lookup tables of the used 
F-16 model do not depend on Mach number. The only difference between the two 
flight conditions is therefore the dynamic pressure caused by a difference in air 
density. The resulting effect on the safe maneuver set can be observed by comparing 
Fig. |5(b)| and Fig. |6(b)| The dynamic pressure at 10000m and 150 m/s is about 75% 
of flying at Om and 100 m/s. The same ratio can be observed from the comparison 
of the maneuver sets shown in figure [6(a)| and figure [6(b)] 
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(a) Vt = 60 m/s, altitude = Om (b) Vt = 150 m/s, altitude = Om 

Fig. 5 Safe maneuver envelopes for two flight conditions with different dynamic pressure. 




(a) Vt = 100 m/s, altitude = Om 



(b) Vt = 150 m/s, altitude = lOOOOm 



Fig. 6 Safe maneuver envelopes for two flight conditions with dynamic pressure of 6125 
Nm-2 and 4643 Nm-2. 



5 Conclusions and Recommendations 

In this paper a definition of maneuvering and dynamic flight envelope was given 
based on the dynamics of an aircraft. Furthermore, a method to determine this enve- 
lope based on reachable sets was formulated and applied to a nonlinear high-fidelity 
model of an F-16 aircraft. The safe maneuvering envelope results agree with what 
is expected from flight dynamics knowledge. Especially for general aviation and 
commercial aircraft, it would be interesting to investigate whether the full enve- 
lope determination problem can be split into fast, and slow dynamics by means of 
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time-scale separation arguments. This would simplify the dynamic envelope prob- 
lem into five and three dimensional subproblems which are more computationally 
tractable than the original problem. 
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Modelica Landing Gear Modelling 
and On-Ground Trajectory Tracking 
with Sliding Mode Control 



Fabrizio Re 



Abstract. A control system for an aircraft taxiing on ground based on sliding mode 
has been developed. The controller is capable to track the trajectory assigned in 
terms of longitudinal velocity and yaw rate and to drive an aircraft equipped with 
electric motors in the main gear as well as conventional brakes and nose gear steer- 
ing. In addition, it can successfully handle saturation of the actuators. The algorithm 
is shown to be robust against parameter uncertainties (e.g. aircraft mass) as well as 
low friction coefficients at the interface tyre-ground. In order to test the tracking 
controller, an accurate virtual aircraft model has been designed in Modelica, with 
particular attention to the landing gears. 



1 Introduction 

In recent years, a great deal of attention in aircraft transportation research has fo- 
cused on reduction of noise and pollution in airports. One specific research topic 
involves the possibility of driving the aircraft by electric motors integrated in the 
landing gears instead of the jet engines while taxiing and manoeuvring on the 
ground. This configuration should be complemented by an on-ground control sys- 
tem capable of driving the aircraft autonomously by tracking an assigned trajec- 
tory. In fact, while automated flight controls are an established reality and advanced 
functions keep being investigated and marketed, the aircraft on ground is still con- 
trolled manually by the pilot to a large extent; automated ground controls, if at all 
present, offer very limited features. Indeed, a robust ground speed and heading con- 
trol system can offer interesting benefits, like for instance insensitivity to wind gusts 
while taxiing. In a broader perspective, automated trajectory planning and tracking 
through an on-ground autopilot function could free the pilot from driving and let him 
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concentrate on pre-flight or post-flight activity (tower communication, preliminary 
controls, etc.); even more importantly, such a system could become part of a fully 
automated airport ground control, which might improve airport capacity and safety. 

Research on the topic of aircraft-on-ground dynamics and control has not been 
extensive so far. Rankin et al. IfTSl [T4l [T5l have studied the lateral dynamics of a 
mid-size commercial aircraft comprehensively; they carried out bifurcation analy- 
ses to investigate the aircraft cornering behaviour in relation to longitudinal speed, 
steering angle at the nose wheel, steering rate, tyre-ground friction and position of 
the centre of gravity. Roos et al. llT6l [TTl [TSl and Biannic et al. Q propose control 
algorithms based on nonlinear or linear parameter- variable (LPV) aircraft models 
and anti-wind-up compensators. Duprez et al. 13] showed that nonlinear inversion 
and linear control techniques are not sufficient for yaw control at very low speeds 
because the system is highly nonlinear due to high sideslips and secondary tyre 
effects; moreover, errors become important if compared to measured or estimated 
quantities (e.g. lateral velocity), causing a need for robust control. 

On the other side, the problem faced in this paper is not really different from a 
vehicle dynamics problem, hence it is appropriate to review the literature on this 
field. Research on autonomous road vehicles is way more advanced, reaching as far 
as to predictive control applications pl and optimal control of overactuated systems 
through control allocation |21. Solea and Nunes 121,1 propose a trajectory planning 
and tracking system for an autonomous vehicle based on sliding mode control. 

This paper shows an approach to designing an automatic aircraft-on-ground tra- 
jectory tracking control based on sliding mode. The attention will focus on the inner 
loop controller receiving kinematic inputs (e.g. velocities and/or accelerations) and 
commanding the actuators accordingly. Such a system can be operated by the pilot 
himself or can be coupled with an outer-loop trajectory planning controller that re- 
ceives trajectory data and calculates the required kinematics. First of all, a model of 
the real aircraft has been realised as a virtual test-rig for the control validation. The 
landing gear and particularly the tyres have been modelled precisely to enhance the 
realism of this virtual aircraft. The development of this model with the Modelica 
commercial compiler Dymola will be described in section |2] Section |3] illustrates 
the control system architecture and design. Some simulation results showing the be- 
haviour of the controlled aircraft will be presented in sectionH) Finally, conclusions 
and the outlook of this work will be discussed in section|5] 



2 Virtual Aircraft 

Since 1995, the DLR Institute of Robotics and Mechatronics has constantly been 
developing a Modelica Flight Dynamics Library (FDL) [91 that allows aircraft mod- 
elling in a realistic world environment at different levels of detail. The FDL makes 
use of the open modelling language Modelica [63 and has already been used in a 
number of internal and international research projects (e.g. ifTOl . [[SI, fT9\ . Q). 

In the first versions of the FDL, the landing gear was modelled in a rather simple 
way, by means of an interaction of basic forces between aircraft and ground. This 
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part has been improved in the present work by modelling a proper landing gear with 
rolling wheels. 

A wheel model has been used together with elements of the Modelica Multibody 
Library to build a model of a whole landing gear. Three variants have been realised: 

• steerable two-wheel bogie without brakes, for use as nose gear; 

• fixed (non-steerable) two-wheel bogie with brakes, for use as main gear on small 
and medium aircraft; 

• fixed four-wheel bogie with brakes, for use as main gear on large aircraft. 

The vertical top end of the gear structure is connected to the airframe model through 
Modelica Multibody interfaces, which represent physical connections between tridi- 
mensional objects with exchange of forces and torques. A suspension and a damper 
are also modelled between the gear structure and the body interface, with a simpli- 
fied stopper to limit the suspension travel within the allowed range. All dimensional 
parameters can be adjusted to model landing gears of different sizes. A Bearing 
Friction block from the standard Modelica Rotational Library [6| is connected to 
each wheel to model the frictional effects in the axle supports. In addition, there is 
a connector to the avionic bus for data exchange with the avionic layer. 

The steerable gear features a rotational joint and an Impressed Position element 
to rotate the whole bogie around the vertical axis. The steering command given by 
the pilot is processed through a first-order lowpass filter to reproduce the delays of 
real steering systems. This part may be replaced with a model of a real steering 
actuator for a more accurate dynamic simulation. In the brakeable gears instead, 
brake elements from the standard Modelica Rotational Library are provided for each 
wheel. In addition, a simple ABS model was developed, appropriately limiting the 
braking force when the longitudinal slip exceeds 15% to avoid wheel blocking. 

For the work described in this paper, the midsize aircraft configuration with two- 
wheel main gears has been used. 

A Modelica tyre model package ll23l already available at DLR was adapted to 
this work. This package features a semi-physical tyre model based on parametrized 
friction coefficients and their dependency on the slip velocities. The longitudinal 
and lateral forces are a function of the tyre slips in both longitudinal and lateral 
direction as well as the camber angle and the normal force at the contact patch. 
Those functions can be varied through several external parameters, such as e.g. fric- 
tion coefficients in different predefined conditions (e.g. at zero longitudinal slip, at 
large longitudinal slip, etc.). The tyre model has been tuned by means of compre- 
hensive experimental data on aircraft tyres provided by a tyre manufacturer. For this 
purpose, a test rig was modelled in Modelica to replicate those experiments, e.g. 
rolling wheel with given speed and slip angle imposed. The tyre model parameters 
were varied to match the simulation results with the experimental data available as 
closely as possible. 

The parameters used in the wheel model include masses, inertias, dimensions 
as well as tyre model parameters and are arranged in data sheets. The gear model 
included in the FDL already contains some real data sets available by default, like 
nose and main gear wheels of a mid-range and long-range aircraft respectively. The 
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user can easily change the parameters to those of any other aircraft by compiling a 
blank data sheet available as template in the library. 

The terrain model of the FDL was enhanced with a basic ground model. It is 
possible to define a distance from the EGM96 geoid surface and a normal vector to 
define the ground slope. In the basic version, these quantities are constant parame- 
ters which are set at the beginning of the simulation, resulting in a simple, smooth 
ground with constant slope. 

As regards the ground driving system, a simple model of an electric motor with 
an ideal characteristic was designed. The motor produces a constant torque below 
a certain rotational speed, and works at constant power above that speed. The input 
is a real number in the range [0; 1] which proportionally controls the amount of 
torque produced, being = no torque and 1 = maximum torque available at the 
current speed. One motor model has been connected to each of the main gear wheels; 
thus the whole aircraft model features four motor models which are independent of 
each other. A driving/braking controller is provided for each gear. It receives the 
commanded driving/braking moments from the on-ground control system described 
further on and regulates the electric moments and the brakes of the two gear wheels. 
The driving/braking effort is equally divided between the two wheels of each gear. 

The steering system has been modelled in a simple way as an integrator of the 
rate of steering angle commanded by the on-ground controller, as the standard input 
of a real steering system is assumed to be the steering rate (commanding the position 
of the servovalve, which in turn regulates the hydraulic flow rate, hence the steering 
rate). This model also features a first-order delay to account for the delayed response 
of the real system. 

3 On-Ground Controller 

The control system shall be capable to handle critical on-ground situations, since 
it will be used as global control system for the complete ground mission (landing, 
turn-off, taxiing, U-turning, turn-on, take-off). While it can be argued that a sim- 
ple linear model should suffice for standard taxiing, situations occur in which linear 
approximations no longer hold. This is the case at low speeds or large steering an- 
gles t3J; also, it can be expected in runway high-speed manoeuvres (e.g. turn-on, 
turn-off) in poor weather conditions. In addition, the behaviour of the aircraft on 
the ground is influenced by external factors (e.g. aerodynamics) and a number of 
parameters that are variable even during the same mission (e.g. tyre behaviour, taxi- 
way surface, variable aircraft mass), therefore the control system must be robust 
against both unmodelled dynamics and parameter uncertainties. 

For these reasons, and also in order to keep the computational effort at a low level, 
a control architecture has been chosen based on a feedforward nonlinear controller 
and a feedback sliding mode control. This kind of control is known for offering 
high robustness in spite of a quite simple mathematical structure dlSOl . llT2ll ). The 
control architecture is shown in Fig. [l] It features a feedforward controller based 
on a nonlinear aircraft model, a feedback sliding mode control that adds robustness 
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Fig. 1 On-Ground Control Architecture 

against uncertainties, and an anti-windup limiter that handles actuator or tyre satu- 
ration. The system inputs are the requested longitudinal velocity Vx.des and yaw rate 
^des- The outputs of the controller are the two driving/braking moments M/, M, for 
left and right wheel, and the steering angle rate 8 at the front gear. The actual steer- 
ing angle 5 and the velocity vector vcg referred to the centre of gravity (CoG) and 
containing Vx, Vy, i/A are then fed back from the virtual aircraft to the controller. 



3.1 Feedforward Controller 

The feedforward controller is based on an inverse nonlinear aircraft model. Vertical 
dynamics and aerodynamics are neglected here; their effect will be compensated 
by the feedback control. Since the wheels of each gear are relatively near to each 
other, it is reasonable to assume that their behaviour will be approximately the same. 
Hence only one wheel is modelled in each landing gear. The whole model will then 
have three wheels, one in the front and two in the rear. The dynamic equations of 
the aircraft on ground are: 



m 



{vx - Vy i/a) = Fx,cG = Fxf cos 5 - Fyf sin 5 + Fxi 
m {vy + Vx^lf) = Fy_cG = Fxf sin d + Fyf cos 5 + Fyi 
J-xif = M,_cG = bf {Fxf sin 5 + Fyf cos 5) 



Fyr 



(1) 



where v^, v,., ^f are the longitudinal and lateral velocity referred to the CoG and the 
yaw rate in an aircraft- fixed reference system; Fx.cg, Fy^cG, M,cg are the aircraft 
longitudinal resp. lateral force on the CoG and the yaw moment; Fxf^ Fyf are the 
longitudinal resp. lateral force on the front gear wheel; the same applies for the left 
main wheel (with index /) and right main wheel (with index r). The mass m and the 
moment of inertia J- around the vertical axis are assumed known, bf and br are the 
longitudinal distances of the nose gear resp. main gears to the CoG; a is the lateral 
distance of each main gear to the CoG. In addition to eq. ([T]l, a tyre model with 
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Fig. 2 Coefficient J]m- as function of Vx and 5 



relationships between tyre forces and kinematics is needed to close the equation 
system. A simplified version of Pacejka's Magic Formula ifTTI has been used here. 

The aircraft longitudinal speed v, and yaw rate \j/ are the inputs of the feedfor- 
ward controller and coincide with v^jes and ij/des- Recall that, in a road vehicle with 
only one steering axle, the absolute velocity and the yaw rate are coupled fT), so 
Vy is already determined here. As they are imposed externally, it follows that their 
derivatives in eq. ([T]) are known. The outputs of the feedforward controller are the 
two driving (braking) moments M/, Mr and the variation of front steering angle 5. 

Since the system is overactuated, additional equations are needed. The required 
yaw moment M^cc, resulting directly from i//, can be generated through both the 
steering system and differential driving moments on the gear wheels, so a strategy 
must be defined to split it among the actuators. It is firstly noted that increasing the 
steering angle 5 has a little effect on the yaw moment at low longitudinal speeds. As 
the longitudinal speed decreases, the yaw moment is more efficiently generated by 
differential driving moments between left and right wheel. Secondly, the greater the 
steering angle 5 already is, the less effective an additional increase of d is in gener- 
ating yaw moment. This is obvious from the yaw equation of the on-ground vehicle 
([T]), where the term Fyf cos 5 decreases for increasing 5 while Fyf will eventually 
remain constant or even decrease because tyre saturation has occurred. Finally, the 
achievable steering angle is limited, therefore an increase of 5 in the feedforward 
controller should be prevented once the maximum steering angle has been reached, 
as only differential moments can produce additional yaw moment in this condition. 
Based on these considerations, an appropriate continuous function rjM- ~ f{vx,5) 
is defined, with < T]m- < 1 . It expresses which part of the needed yaw moment 
will be generated by differential moments; the part generated by the steering system 
is then 1 — tjm-- As shown in Fig. [21 tjm- is 1 for v.v = and decreases monotonically 
to 0.1 for v.v -^ °° when \5\ is smaller than 45°. For |5| > 45°, rjM- increases; it 
becomes 1 regardless of v^ for the assumed maximum steering angle of 65°. 

Starting from tjm- ■ M^cg ^nd considering the wheel radius and the distances of 
the main gears to the CoG, the differential motor moment M^,yy that generates the 
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yaw moment is calculated. The requested longitudinal force is completely generated 
by the wheel driving moments; the total sum of the driving moments 2 • Mi„„g is 
calculated by dividing the longitudinal force by the wheel radius. Finally, the motor 

moment on each wheel is: 

M,=M,o„g+M,,iff 

Mr=M,o„g-Mdiff 

The feedforward controller is implemented as a Modelica obj ect. The Modelica parser 
automatically solves the system for the unknowns 5, M/, Mr without need to rear- 
range the equations manually; 5 is then directly available by differentiating 5. 



3.2 Sliding Mode Feedback Controller 

Let Vx{t) = Vx — Vxjes and \j/{t) = y/— ^cies be the tracking errors of the state vari- 
ables to be controlled. We define two surfaces for both variables (see also ll20ll ): 



Sv^=Vx + Xy,Vx=0 

Assuming the initial conditions v.i (0), ^f{0) = and Va(0), ^f{0) = 0, it is straightfor- 
ward to show that Vx{t), i//(f ) = is the unique solution of eq. Q. The tracking prob- 
lem is then equivalent to that of finding a control law such that 5,„ , ■Sy, = V t > 0. 
The situation where Sv^,s^j/ = is called the sUding condition. However, the real 
plant could differ from the internal model due to modelling errors (e.g. aerody- 
namic effects) and parametric uncertainties (e.g. different mass). Hence the control 
law must first provide that .? — > 0, that is, that all trajectories must point towards the 
sliding surface s; once 5 = is reached, that s = 0. The former requirement can be 
formalized as follows I20i : 

~/<-n^\ (4) 

with the strictly positive constant k. A control law of the form u — —k sgn [s) guar- 
antees that the sliding condition is reached in a finite time t <s{t = 0) jk; this can 
be proved by integrating eq. |4]in time. Once the sliding surface has been reached 
(s = 0), the dynamics is given by eq. [3j that is, the tracking errors will tend to 
zero exponentially with the time constants A,;^\A^'. However, such a control law 
would result in practice in high-frequency chattering around the the sliding condi- 
tion. Therefore, the sign function is replaced with the following saturation function: 

sat(./0) = ^/'^ '/IV'^^I<1 

[sgn (5/^) otherwise 

where O is a strictly positive constant. The control law guarantees now that the 
boundary layer B(t) ~ {x: |5(x,f)| < <&} is reached in a finite time smaller than 
5 (f = 0) jk, still allowing good tracking within a predefined tolerance for suffi- 
ciently small fp; more precisely, \x\ < 20 \/t >0 holds for any trajectory starting 
inside B{t), where x is the tracking error of the controlled state variable. 
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In the present problem, two constants 0,,, and 0^^ must be defined for the two 
variables tracked. For this application, they have both been set as 0.1. While such 
a precision requirement is reasonable on the longitudinal velocity error where the 
absolute values are in an order of magnitude of 1-10, it might arguably be too large 
for the yaw rate. However, the simulation results show that very good tracking can 
nonetheless be achieved; in fact, a smaller 0y, would affect the computational time 
seriously without adding any noticeable benefit. Both Ay,, A|j, have been set as 3. 
This means a time constant of 0.33 s for both tracked variables, which is deemed 
appropriate considering the system characteristics. 

The control laws of the feedback part are then: 

Miong^ -kM sax {sy J 
Mdiff = -nm-km sat [s^) (5) 

5 = -(l-T]M,)A:^sat(i'^) 

where r\M- is the coefficient in Fig. [J] km, kg are the gains of the control laws. As ex- 
plained previously, they determine the time in which the sliding condition is reached. 
It should be noted that the saturation functions cannot be greater than 1 in modulus, 
so it is not significant to set gains much greater than the actuator saturation values; 
on the other hand, the feedback control must be able to correct a possibly inade- 
quate feedforward control, which might be at saturation in the opposite direction in 
the worst case. For this reason, the first two control laws, which command a motor 
moment, have the same km = 7000 which is twice the maximum motor moment 
assumed of 3500 Nm; the third control law, responsible for the front steering, has 
kg = 3 being 1.5 rad/s the assumed maximum turning rate of the steering system. 

The left and right driving moments resulting from the first two control laws are 
determined according to eq. ([2|. 

3.3 Anti Wind-Up Limiter 

With the control configuration described so far, the derivatives of the control inputs 
Vx and ^f should be limited through an appropriate rate limiter for two reasons. On 
one hand, the control system is still not able to deal with the saturation of the actua- 
tors. Modelica can only solve the feedforward controller equations as a closed equa- 
tion system; actuator saturation cannot be implemented there because the equation 
system would have no exact solution for certain inputs. In the configuration without 
rate limiting, inputs that would be impossible in reality because of actuator satura- 
tion are still valid inputs of the feedforward controller and cause its states to diverge 
significantly from the real aircraft ones. This, together with the logic of the sliding 
mode control commanding large controls to compensate that difference, causes an 
effect analogue to wind-up in integrators C ll22l . Il20l ). This is problematic when the 
control input is not monotone, because a sign change in the control input rate has to 
bring the model states back to the real states first and has therefore only a delayed 
effect on the real system. On the other hand, it is important to ensure that the con- 
trolled system always remains in a stable region. This means in particular that the 
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tyre slip must never be greater than the slip corresponding to the maximum force in 
both longitudinal and lateral direction. A solution is to limit the rate, i.e. the deriva- 
tive, of the control inputs if the inputs themselves diverge from the system states 
beyond a certain threshold. In this way, the model states can match the real ones as 
closely as possible and no unrealistic inputs can be passed to the control system. 

The threshold variable for the longitudinal direction is chosen to be the differ- 
ence between real and model longitudinal speed v^. This is immediately available as 
measurement (ground speed) in the real system. For the lateral direction, we define: 

qu., = sgn (5) Gy.f + I 2.5 (5 - 5o sgn 5) otherwise 

where 5 is the actual steering angle and Gyj is the lateral slip at the front tire 
calculated as the ratio of tyre lateral velocity to tyre absolute velocity: Gyj = 

^yjl xh'x f'^'^'yf- These velocities are available through either measurements or es- 
timates. It should be noted that this anti wind-up limiter only needs to look at the 
slip conditions at the front tyre to ensure permanence in the stable region. If the rear 
tyres should lose grip, the real yaw rate would become quickly greater than the one 
in the controller model, and the sliding mode feedback controller would immedi- 
ately contrast this by countersteering and commanding a moment difference in the 
opposite direction, much like an ESP system in a road vehicle. 

The anti wind-up limiter regulates the derivative of the outputs (which will be 
the commanded states of the feedforward controller) as a function of the inputs, 
their derivatives and their respective threshold variable. If the threshold variable 
exceeds the allowed value, the output rate is decreased and follows the rate of the 
real system, which is supposed to be the maximum feasible rate since the model and 
the real system were diverging. If the threshold variable is below the allowed value, 
the output rate is equal to the input rate to guarantee tracking precision; if there is a 
difference between feedforward controller states and real system states due to prior 
rate limitation, the rate limiter no longer intervenes, and if necessary, the output rate 
is additionally increased resp. decreased to reduce the state difference to zero. 

This anti wind-up arrangement results in limiting the aircraft longitudinal force 
resp. the yawing moment whenever the real system cannot physically achieve them. 
However, if the pilot or the outer-loop controller commands a different velocity or 
yaw rate whose derivatives are smaller in modulus or change sign, the anti wind-up 
limiter no longer intervenes. In this sense, this limiter does not bring additional risk 
of pilot induced oscillations, nor does it limit or delay the pilot inputs in any way if 
no saturation has occurred. 



4 Simulation Results 

A virtual aircraft model of a mid-size aircraft has been built in Dymola 7.1 us- 
ing the FDL (section O and electrical motors have been added to each of the four 
rear wheels. The tracking controller composed of a nonlinear feedforward controller 
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Table 1 Different features of the simulations carried out 

Simulation Features 



#1 
#2 
#3 



Mass = 56900 kg, friction coeff. = 0.8 * nominal 
Mass = 66900 kg, friction coeff. = 1 * nominal 
Mass = 76900 kg, friction coeff. = 0.6 * nominal 



and a sliding mode feedback controller has been connected appropriately and three 
different simulations have been carried out. To test the controller robustness, the 
aircraft mass and the tyre-ground friction coefficients stored in the virtual aircraft 
model were different in each simulation, while the aircraft model in the feedfor- 
ward controller remained unchanged (see table [1]). The inputs are predefined time- 
dependent profiles of longitudinal velocity and yaw rate. The simulations were run 
in Dymola using the DASSL algorithm with variable time step. 

It can be seen from the longitudinal velocity diagram (Fig. O that the controller 
accelerates the aircraft as much as possible to reach the requested v^; the acceleration 
differences are due to the different mass in each case. The braking phases track the 
requested deceleration precisely in all cases since the brakes are never saturated. 

The yaw rate is tracked with very good precision in all simulations. The curves in 
the diagram (Fig.|4| mostly overlap with the requested yaw rate. During the last cor- 
ner, the simulation with the highest mass and the smallest friction coefficients shows 
a clear delay in reaching the requested yaw rate because the front wheels are satu- 
rated and yaw rate limiting occurs to keep the aircraft in the condition of maximum 
lateral force. This indeed assures a stable operation; on the other side, it affects the 
precision of the trajectory tracking because the cornering radius is reached later than 
requested. This circumstance shall be handled at the outer control loop level which 
assigns the requested inputs, that is the pilot or the autopilot system; they must 




Fig. 3 Longitudinal Velocity - solid line = input velocity; dotted line = simulation #1 ; dashed- 
dotted line = simulation #2; dashed line = simulation #3 
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Fig. 4 Yaw Rate - solid line = input velocity; dotted line = simulation #1; dashed-dotted line 
= simulation #2; dashed line = simulation #3 



recognize (by feedback from the rate limiters) that the physical limits of the system 
have been reached and recalculate new inputs to be able to follow the trajectory. 

5 Conclusions 

An aircraft on-ground control system based on sliding mode has been designed in 
Modelica. The controller allows precise tracking of assigned longitudinal velocity 
and yaw rate by controlling the steering angle, the braking system and electric mo- 
tors connected to the main gear wheels; at the same time, it takes actuator saturation 
and physical limits of the system into account. It has been shown through simula- 
tion results that the controller is robust against different aircraft mass and varying 
tyre-ground friction coefficients. 

During this work, new Modelica landing gear and tyre-ground interaction models 
have been developed as an extension to the DLR Flight Dynamics Library. The 
new parts allow detailed simulation of an aircraft moving on ground with particular 
regard to the dynamic aspect. The new models can be used as sub-components in 
larger Modelica models, thus allowing global, multi-disciplinary aircraft simulation. 
For instance, the landing shock and its influence on the aircraft structure can be 
analysed in a flexible aircraft model; also, side forces acting on the landing gears 
can be quantified, and their components can be dimensioned accordingly. 
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Obstacle Avoidance Strategy for Micro Aerial 
Vehicle 

Cezary Kownacki 



Abstract. Obstacle avoidance of Micro Aerial Vehicle (MAV) in urban environ- 
ment is the most complex, difficult and essential part of the autonomous flight 
problems. The paper presents a simple ad-hoc strategy using a pair of miniature 
laser rangefinders (i.e. MLR 100) and two PIDs cooperating with an obstacle 
avoidance controller. The strategy can be realized as an additional routine inte- 
grated with the autopilot's firmware (i.e. MP2128^^'''). The main advantage of the 
proposed strategy is simplicity of its implementation in small-sized MAVs and its 
power efficiency. All previous works, especially the vision-based ones require high 
performance microprocessors which is an important limitation when applying on 
real MAVs. On the other hand, the autonomous controller, which is based on optic 
flow sensors, is easy to implement on even tiny MAVs, but optic flow sensors 
require applicable level of contrast variation, so their performance is strongly sen- 
sitive to weather conditions. The proposed idea of the autonomous obstacle 
avoidance system in urban environment was simulated using MATLAB - SIMU- 
LINK software. In the real flight all computations and controls will be realized by 
the advanced autopilot, hence the rest of autonomous control and complex flight 
dynamics are not included in the simulation. The assumption allows to spot a more 
focused attention on the obstacle avoidance problem and a simpler model of the 
MAV can be used in the simulation. The results presenting the 2D trajectories 
confirm that the effectiveness and safety of the proposed strategy of obstacle 
avoidance is attainable during the real flight in streets' canyons. 

Keywords: obstacle avoidance, autonomous flight, autopilot, streets' canyons, 
miniature aerial vehicle laser rangefinder. 

1 Introduction 

The main purpose of research on autonomous MAVs is to achieve the best 
autonomy and flight performance in unknown environments as much as possible. 
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The urban environment is the most difficult and uncertain area where MAVs can 
perform flights. The assurance and rapid obstacle detection are highly expected 
because of the significant variability and variety of objects appearing around the 
flight path. Flying over an urban environment also requires the waypoint navigation 
in order to planning paths which fulfills the goals of the flight and it is essential to 
the diverse missions of MAVs. Therefore the robustness of the autonomous flight is 
depended on navigation algorithms and autonomous control laws. 

Many researchers work on improving the effectiveness of the autonomous sys- 
tems used in MAVs. Most of them focused on the vision-based systems and digital 
image processing [1, 3, 5]. Vision-based systems allow observing a visible area 
around the MAVs and it makes possible to create a 3D or 2D map. The map can be 
further utilized by the path planners [12].But the obvious disadvantage lies in the 
complexity of the image processing routines, which require efficient DSPs with 
high power consumption. In turns it shortens the flight duration. Increasing the 
capacity of batteries also increases the weight of the whole system. Moreover, the 
image stabilization is an additional problem. Although a mechanical stabilizer can 
help, it means more electronics and more computations. Vision-based systems are 
the future of autonomous flight control systems, but today's technology still fails to 
meet the specifications of the tiny MAVs. 

Some of the recent researches on the autonomous systems presented the control 
strategy based on optic flow sensors and it gives satisfying results. However [10, 
11, 12], most demonstrations required special patterns as landmarks in order to 
guarantee sufficient visual features to estimate safe flight path. If the variation of 
the contrast is too low the control strategy based on optic flow will be useless and 
collisions will be unavoidable. That's why optic flow sensors performance is 
strongly sensitive to weather conditions and illumination. 

Designing of miniature fixed-wing or delta-wing aerial vehicles requires to be 
light-weighted and low-power consumption. Hence it requires a power efficiency of 
the on-board equipment realizing autonomous flight control. On the other hand, 
complex algorithms of the autonomous guidance and obstacle avoidance must 
involve high performance microprocessors, which are necessary in rapid computing 
for a safe and sound flight paths. Most of the developed algorithms are only offline 
experiments, computer simulations or online experiments carried out in selected 
environments without verifications in different flight scenarios or weather condi- 
tions [1-11]. So the challenge is to design an effectively autonomous flight control 
system which will guarantee a reliable autonomous flight in various real world 
scenarios and it could be realized by available equipment [12]. 

We propose an strategy of autonomous obstacle avoidance, while micro aerial 
vehicle flies in streets' canyons, which can be realized on readily available 
equipment: two miniature laser rangefinders MRLIOO (from Aerius Photonics) and 
advanced autopilot MP2128'"^'' (from MicroPilot). We couldn't use laser scanner, 
because it is too heavy to place them in our airframe (wingspan: 1245 mm and total 
weight: 1200 gram). Some strategies of obstacle avoidance use a single laser 
rangefinder, but such configuration disables possibility of flight in streets' canyons 



Obstacle Avoidance Strategy for Micro Aerial Vehicle 119 

[12]. So we decided to use configuration, which enables autonomous flight in can- 
yons, but instead of optic flow sensors we choose laser rangefinders [12]. All nec- 
essary computations can be realized by the autopilot, so any additional electronics 
would not be needed. The manufacturer of the autopilot delivers a development tool 
called XTENDER, which allows modifying the autopilot firmware with additional 
user code. Although the strategy presented in this paper is validated only in simu- 
lations, it proves that there is a great chance to introduce it into real MAV. And it 
will be the next step of our research. 

2 Obstacle Avoidance Strategy 
2.1 Hardware and Configuration 

Two miniature laser rangefinders (LRF) are the base of proposed autonomous 
obstacle avoidance strategy (Figure 1). They will measure ranges between MAV 
and objects lying on both sides of the vehicle track. Controlling these ranges by 
PlDs' loops and obstacle avoidance controller will allow avoiding obstacles and 
continuing flight in street canyon [12]. The great advantages of miniature laser 
rangefinder are its features predisposing it as perfect sensor which can be used in 
miniature aerial vehicle. Features of the miniature laser rangefinder are presented in 
Table 1. 

Table 1 Features of miniature laser rangefinder MRLIOO 



Feature Value 

Weight 26 g 

Power rating <400mW 

Size l,25"xl,5"xl,6" 

Pulse repetition 500Hz 

Resolution <0,2 m 

Range ~0.1mto>100m 

Divergence angle 10x10 mrad 

Output UART 

Filters averaging and 

median filters 

The most important features are small size and weight, power efficiency, high 
repetition rate and UART serial port which is indispensable for measured data 
transmission. Also detectable maximum range fits specific conditions of obstacle 
avoidance during flight in streets' canyons. 
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Of course there are many other sensors which can be used in obstacle avoidance 
system, but their possibilities make them losing out with the laser rangefinder. 
Miniature low resolution cameras are smaller and lighter, but image processing 
routines running on energy efficient microprocessors, are still insufficient to reduce 
influence of contrast and obstacle illumination conditions. The extremely case, 
when always cameras fail, is a night flight. Also optic flow sensors are sensitive to 
contrast and weather conditions. For instance they are unable to detect range to 
obstacle properly while MAV is flying nearby uniform building wall with no ap- 
plicable contrast differences. Such situation can happen in urban environment 
regularly. The last kind of sensor, which is frequently used in robotics, is an ultra- 
sonic sonar. Its outdoor version has similar weight and dimensions as the laser 
rangefinder. The main advantage of ultrasonic sensors is a wide beam cone, so the 
scanned area is much bigger than the laser rangefinder measurement at single point. 
In the other hand scanning range is limited up to a few meters against more than 100 
meters achieved by the laser rangefinder. If MAV flies fast, a few meters scanning 
range would not be enough to ensure space for turn. Hence in our opinion the laser 
rangefinders are the best sensors for our purpose at the moment which fulfill re- 
quirements of obstacle detection in urban environment. 




Fig. 1 Miniature laser rangefinder MRLIOO from AERIUS. 

Configuration of miniature laser rangefinders is a significant aspect of the system 
reliability and usability. In our opinion the sensors configuration, which enables 
flight possibility in streets' canyons, is as follows: both laser beams are placed 
tangent to the MAV plane and they are forward looking, but swept back from nose 
by specified angle. So two laser beams will create "V" shape (Figure 2). 

The configuration satisfies both aspects of autonomous flight in urban envi- 
ronment: flight in streets' canyons and obstacle avoidance, because laser range- 
finders scan not only area at MAV front but also on both sides in directions of 
potential flight. Angle between laser beams can be evaluated from formula: 



a = 2 • arctan 

where: R- minimum turn radius 



, — ^-[rad] (1) 

yiSR 3 
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Fig. 2 The "V" shape created by laser beams of miniature laser rangefinders: a - angle be- 
tween laser beams, R ~ minimum turn radius. 



Thanks to the chosen configuration, the a angle is independent from flight pa- 
rameters such as speed or minimum turn radius. 

Typical use of ultrasonic or laser rangefinders is scanning and sweeping the en- 
vironment while the mobile robot is moving. Such strategy allows rendering the 
environment occupancy grid map and thus it is possible to generate the safe tra- 
jectory. While occupancy grid map calculations are simple for 2D environments, 
things goes worse for 3D environments, which are typical for aerial vehicles. Di- 
mensions and sizes of occupancy grid map are increasing since MAV 3D operating 
zone is much bigger than 2D zone typical for mobile robot. Even if the MAV flight 
zone could be scanned only in 2D, the laser sensor must be stabilized by special 
gimbals' platform, so beam targeting also won't be easy. 

That's why laser rangefinders will be fixed to the MAV body. Thus measured 
ranges should be independent from MAV body frame orientation, what is essential 
for determining true range to obstacle. Hence we have to transform coordinates 




Fig. 3 MAV flight in street canyon, (|) - roll angle, Dyl - NED frame y coordinate of obstacle 
on the left, Dyr - NED frame y coordinate of obstacle on the right. 
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Fig. 4 MP2128heli from MicroPilot 



Table 2 Main features of autopilot system MP2128 



Feature 








Value 


Weight with GPS 








28 g 


Power rating 








140 niA @ 6.5V 


Size 








100x40x15 mm 


selectable inner loop update rates 








30/60/180 Hz 


user definable PID feedback loops (for camera 


stabilization etc) 


8 


user definable table lookup functions 






8 


autonomous takeoff and landing s 


;upported by AGL 


yes 


change altitude at waypoint 








yes 


change airspeed at waypoint 








yes 


user definable holding patterns 








yes 


user definable error handlers (lost 


i of GPS, 


low 


battery etc.) 


yes 


servo resolution 








libit 


servo update rate 








50 - 200 Hz 


altimeter maximum altitude 








12,000m 


3 axis accelero meters 








2g 


maximum angular rate 








150° per second 


attitude update rate 








200 Hz 


12 state Kalman filter 








yes 


GPS update rate 








4Hz 


supports DGPS accuracy 








yes 


waypoints 








1000 



of obstacle location from MAY body frame to NED frame (North East Down) 
(Figure 3). Required values of actual roll and actual pitch angles are available from 
autopilot gyros. 

Reliability of proposed autonomous obstacle avoidance strategy strongly relies 
on predictable flight control, so the high quality autopilot system should be used. 
Creating individual autopilot for our purpose seems be pointless, because probably 
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it would take much time and final result couldn't be as good as available commer- 
cial autopilot from MicroPilot or Procerus. Table 2 presents main features of 
MP2128'"''' (Figure 4), the most advanced autopilot system from MicroPilot, which 
will be suitable enough for our purpose. 

2.2 Diagram of Obstacle Avoidance Control System 

Usage of laser rangefinders in the autonomous obstacle avoidance strategy requires 
introducing adequate control algorithm. We propose the algorithm, which is based 
on two PIDs' loops cooperating with obstacle avoidance controller. The routine of 
the algorithm will be integrated with the autopilot software responsible for standard 
control laws and waypoint navigation. Obstacle avoidance controller will decide 
about actual priority of each control task i.e. waypoint navigation and obstacle 
avoidance including flight in streets' canyons. Obstacle avoidance controller will be 
able to override standard autopilot controls. 

The diagram of designed autonomous obstacle avoidance system is presented in 
Figure 5. 



Left laser 
rangefmder 



Right laser 
rangefmder 



Filters 



Filters 



^^ Autopilot MP2 128'" 



UART 



GPS, IMU, pressure sensors 



■ autopilot functions, 
- user PID loops, 

■ obstacle avoidance controller. 



H 



Actuator 



RC receiver 

Z 



XBee Pro 2,4GHz transceiver 



LAPTOP - Horizon GCS 



XBee Pro 2,4GHz transceiver 



RC transmitter 



Fig. 5 The diagram of designed autonomous obstacle avoidance system 



GPS and IMU sensors are internal parts of the autopilot and they are used in 
standard control functions and waypoint navigation. GCS (ground control station) 
is communicating with the autopilot via 2,4 GHz radio modem. MIAV can be also 
controlled manually using standard RC equipment. The strategy presented in the 
paper will be only an extension of standard functions of MP2128^'^''. 

Laser rangefinders should measure range between MAV and buildings during a 
flight in streets' canyons. Of course there are many smaller objects i.e. street lights, 
traffic lights, trees, road signs which can't be properly detected by the obstacle 
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avoidance strategy. This is an important disadvantage. Any small obstacle detected 
by laser rangefinders will introduce peaks in ranges sequence similar to random 
disturbances or noise. So it could probably affect on MAV behavior negatively 
making it unstable. We decided to filter out these peaks as useless information. It 
can be achieved by enabling median and averaging filters which are standard fea- 
tures of MLRIOO laser range finder. Hence it requires making an assumption that 
the MAV is able to fly in streets' canyons at patrol altitude. 

2.3 Signal Flow and Processing 

The diagram of signal flow and signal processing is presented in Figure 6. 



Left laser 
rangefinder 



Right laser 
rangefinder 



Autopilot MP2128"' 



Hra,™ lr!,nsrorm.lion Autopilot functions 

-■- , " ^ 



2nd User PID ^ 



Obstacle avoidance 
controller 




Fiamt: Iranafunniilio 



Actuator 



Fig. 6 The diagram of signal flow 



The initial step of signal processing is filtration essential for eliminating range 
peaks generated by small obstacles, wind or other disturbances. It will be done by 
internal filters of laser rangefinder MLRIOO (median filter and averaging filter). 
MLRIOO transmits filtered range values over a serial port, so they are almost ready 
for use. If the MLRIOO internal signal processing become to be inadequate, there 
will be possibility to introduce software filtration realized by the autopilot. But we 
have to keep in mind fact that additional computations are able to slow down 
autopilot operation. 

The next step of signal processing is coordinates frame transformation. Obstacle 
coordinates expressed in body frame can be derived from range value when a angle 
is known (Equation 1). 
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where: D gp- measured range (body frame), 
a - angle between laser beams, 
■Xbf, yBF- obstacle coordinates in body frame. 

Next the body frame obstacle coordinates must be transformed to NED 
(North-East-Down) frame. It allows to determine true range between obstacle and 
MAV. Formula of transformation is given by equation: 



y NED 



V ^NED J 



V 



1 
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^cos^ 








cos^ 


sin^ 







1 





-sin^ 


cos^ 




1 - sin 







cos^ 



V 
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(3) 



/ 



where: xbf, yBF- body frame obstacle coordinates, 

^NED, yNED- NED frame obstacle coordinates, 
(Z> - actual roll angle, 
0- actual pitch angle. 

NED frame coordinates can be used to compute range D j^ed expressed in NED 
frame. 



D' 



NED 



~ V y^NED 



+ y 



NED, 



(4) 



The next step is computing derivative of D med and use it as control signal, what 
makes obstacle avoidance system sensitive to velocity and direction of approaching 
MAV to obstacle. Because if (D NED)ldt equals zero, it would mean that the MAV is 
flying parallel to the obstacle. The maximum value of (Z) NED)ldt presents situation 
when the MAV is flying towards to obstacle, so collision probability increases. 

The final step of signal processing is setting up thresholds which allow ignoring 
obstacles placed further than specified safe range value Dsaje- It also allows ignoring 
obstacles, which MAV are receding from ((D NEoVdl is a positive value (Equation 
5)). Thresholds ensures that relevant component of PID's input signal will equal 
zero (Equation 6). The PID's input signal PIDi„is defined as follows: 



^D ^NED ^safe 
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NED 



D f <0 

safe " 



D f >0 

safe " 
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dt 

d{D*NED) 

dt 
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dt 
>0 



(5) 
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Where: Eo - first component of PID input signal, 
Dsafe - safe range to obstacle, 
D NED- range to obstacle expressed in NED frame, 
Eo - the MAV to obstacle approaching rate- second component of PID 
input signal, 
PIDin - input signal of PID controller. 

Presented definition of PID's input signal makes the obstacle avoidance strategy 
activate only when the MAV is approaching towards or too close to obstacle. Because 
MAV to obstacle approaching rate Eo is proportional to the MAV speed, the obstacle 
avoidance strategy is indirect function of the MAV speed. Whereas PIDs' outputs has 
the sense of desired roll angle with opposite sign for each PID (left PID sign + and 
right PID sign -). 

2.4 Obstacle Avoidance Controller 

The role of the obstacle avoidance controller is to specify what flight control task 
should be considered actually: waypoint navigation or obstacle avoidance. Its input 
signals are outputs of PIDs and actual values of both Dned ranges. The obstacle 
avoidance controller works as a kind of commuting element which switches control 
of desired roll angle between PIDs and autopilot waypoint navigation dependently 
on actual obstacle position. The desired heading angle of delta-wing MAV is a 
function of desired roll angle if^ and desired velocity V*^. Then having constant 
desired velocity V*^ the strategy of desired roll angle control realized by the obstacle 



f = 



avoidance controller is described by Equation 6 (Figure?). 

(l>PlDngh, ^ ^ <t>PIDlef, *^^ \<t>PIDngh, \ * \0PIDlef, \ 
(<t>PlDrigh,*^^ ^PIDlcft=^)^ 

'(t>PIDngh, ^ n (t)p,^,^f, 5t n \(l)p,Ongh, I = \0PlDlef, \ ^ 
^^NEDleft '^^NEDright 

'^PIDrigh, *^^ ^PIDleft ^^^ \^PIDngH, \ = \^PID,ef, \ ^ ^ 
y^NEDleft — ^NEDright y 

(6) 

where: (f'- the MAV desired roll angle, 

^autopilot- desired roll angle from waypoint navigation, 
(ppiDiefr- desired roll angle from left PID, 
^piDright- desired roll angle from right PID, 
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Fig. 7 Control strategy of desired roll angle realized by obstacle avoidance controller 



If outputs of both PIDs are zero, waypoint navigation will control the desired roll 
angle (Figure 7-1). When both of them have two different non-zero absolute values, 
desired roll angle will be calculated as a sum of both PIDs' outputs (Figure 7-4). But 
at the moment it's necessary to mention that (Z>/>/dz<.// is positive desired roll angle 
(right wing down) and ^piorighr is negative desired roll angle (right wing up). So 
0piDieft makes the MAV turning right and ^piDngin makes the MAV turning left. 
Another situation is occurring when only the left PID output is zero, or when both 
PIDs outputs have the same non-zero absolute values and D med value from left 
sensor is greater than D ^ed from right sensor (Figure 7-3 and Figure 7-6). In the 
case the right PID output will control the desired roll angle (?>''. The last one case is 
opposite to the pervious. The left PID output will control the desired roll angle (^'', 
when only the right PID output is zero, or both PIDs outputs have the same non-zero 
absolute values and the D f^ED value from left sensor is smaller than the D f^ED from 
right sensor(Figure 7-2 and Figure 7-5). 

In summary the obstacle avoidance controller controls the desired roll angle 
accordingly to actual state of surrounding obstacles and coordinates of the next 
waypoint. 
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3 Simulation 

3.1 Delta Wing MAV Model 

The simulations of proposed obstacle avoidance strategy were performed in 
MATLAB - SIMULINK environment, because at the moment there is no possi- 
bility to involve light simulator, like it's possible for Kestrel autopilot [12]. Hence 
adequate model of MAV is required for deriving simulated flight paths. We used 
simple delta- wing MAV model which is defined by following equations [6]: 

x = V -cosy/, 
y = V-smy/, 

lJ/ = —tanS, (7) 

r y 

V = ayiV'-V), 

(p = oc^if -<!>)■ 

where: x,y- MAV actual coordinates, 
V- the MAV actual velocity, 
V^- the desired MAV velocity, 
\f/- the MAV actual heading angle, 
^ the actual roll angle, 
^ the desired roll angle, 
a^ ay- time constants of MAV dynamics, 
g - the gravitational constant. 

The model is simple, but it's sufficient for our simulations, which main purpose is 
to present trajectory of MAV flight in different scenarios of surrounding obstacles. 
The model excludes several parameters of MAV flight in the real world, which are 
more dependent on autopilot performance than rather on the obstacle avoidance 
strategy. Creating the individual autopilot and its control software is not the purpose 
of our research. We decided to use commercial MP2128^'^'' autopilot, so we can 
afford to introduce presented simplifications. But the important part of our future 
flight tests will be precise autopilot tuning to ensure good flight performance in 
different weather conditions. 

Our simulations assume that the flight path is controlled by two parameters: 
flight velocity V^ and desired roll angle ^^. We also assumed that the flight velocity 
V*' is a constant and only desired roll angle ^''is controlled by the obstacle avoid- 
ance strategy. 
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3.2 Autopilot Model 

A autopilot model is also required for good simulation performance. Because the 
main purpose of the simulation is presenting only 2D flight paths, autopilot model 
functions were restricted to the waypoint navigation realized by heading angle 
control. The autopilot was modeled by a single PID controller which minimizes the 
track angle error between the actual heading angle and the actual bearing angle 
pointing on the next waypoint. So the model can be formulated as follows: 



V^err^V^Wp-V^^ 



W^ 



WP 



■■ arctan 



ywp-y 



V 



"Vf 



(8) 



^. 



autopilot 



PID(W.,r) 



where: (^j.,.,. - the track angle error, 
I//- the actual heading angle, 

i//wp- the actual bearing angle pointing on the next waypoint WP, 
X, y - actual coordinates of MAV, 
xwp, ywp- coordinates of the next waypoint WP, 
PID - the transitions function of PID, 
^mitopiio t- the desired roll angle. 

The desired roll angle ^aumpUoi is the model output and it is applied when the actual 
range to the nearest obstacle is greater than safe range Dsa/e (Figure 7-1). 
The whole simulation model is presented in figure below. 
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Fig. 8 The simulation model of proposed autonomous obstacle avoidance strategy. 
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Four synthetic maps representing streets' canyons and obstacles were used in 
flight simulations. 

4 Results 

Four maps of environment characterizing different flight scenarios were prepared to 
present the control strategy possibilities. Two of them are concerning the problem 
of flight in streets' canyons. Next two present the problem of obstacle avoidance. In 
each simulation the level flight with initial heading angle was started at the point 
marked as SP. The level flight target was the next waypoint marked as WP. Results 




Fig. 9 The MAV flight path in the first simulation: fight in street canyon. Initial heading 
angle -O", starting point SP - x=50,y=350, the next waypoint WP - x=1000,y=550. 




Fig. 10 The MAV flight path in the second simulation: fight in street canyon. Initial heading 
angle - O", starting point SP - x=50,y=450, the next waypoint WP - x=1000,y=200. 
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Fig. 11 The MAV flight path in the first simulation of obstacle avoidance. Initial heading 
angle - O", starting point SP ~ x=50,y=750, the next waypoint WP - x=1000,y=50. 




Fig. 12 The MAV flight path in the second simulation of obstacle avoidance. Initial heading 
angle - 50", starting point SP - x=50,y=50, the next waypoint WP - x=900,y=600. 



of simulations are presented in Figures 9-12. Obstacles are represented by shapes 
filled with black color. The maps boundaries were treated as obstacles, so MAV 
should also avoid them. Following values of simulations' and models' parameters 
were used: V~=\2 m/s, Dsafe=60 m, (X=i(f, ay =0.5, a^O.5. It also was assumed that 
there was no wind or other external disturbances during simulated flight. 

If we take a look on results presenting flight paths, we will notice that the MAV 
always flies holding a safe range to obstacle. Of course sizes of all obstacles are 
much bigger than MAV size. So the MAV position is represented by single points 
creating the flight paths in all figures. That's why some turns seems to be too sharp 
to perform by delta-wing MAVs. However more details of the obstacle avoidance 
strategy behavior can be determined by observation and analysis of obstacle 
avoidance controller inputs and outputs: D*t^ED, <f , 4>amop\ot and y/. Plots of specified 
signals are presented in figures 13-15. They are concerning the simulated scenario 
of obstacle avoidance from figure 11. 
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Fig. 13 Simulated range signals D ^^o- 
rangefinder. 



Yellow - left laser rangefinder, pink - right laser 




Fig. 14 Simulated (f) signals - desired roll angle. 




Fig. 15 Simulated (4,„,„,„7„, signals 
and \f>~ actual heading angle (pink). 



desired roll angle from waypoint navigation (yellow) 
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Simulated flight path are not the shortest path of possible MAV flight (for ex- 
ample in Figure 11), because in our research we skipped the problem of path op- 
timization. Path planner requires computations which can't be handled by the 
autopilot without performance drop. In some research path planner was design as a 
separate PC application, and computed optimal patch was uploaded to the autopilot 
as waypoints list [12]. So presented path planner can't be treated as a part of 
autonomous system. Absence of path planning function is some disadvantage of 
presented obstacle avoidance strategy. But the main research purpose is to present 
functional idea of obstacle avoidance strategy which would be based on available 
equipment and fit requirements of micro aerial vehicles. 

5 Testing Airframe 

To verify received simulation results we start to prepare a testing airframe based on 
delta wing RC model made with ELAPOR. ELAPOR is a material which gives 
great possibility of introducing any required modifications in airframe body. It's 
also resistant to damages in the case of eventual crash and easy to repair. 

The MP2128'"^'' autopilot, speed controller and standard 35 MHz RC receiver 
were mounted inside the airframe cockpit which is closed with plastic cover pro- 
tecting onboard electronics. XBee Pro 2,4 GHz radio modem, GPS antenna and 
batteries were placed in cavities located in both wings symmetrically. We also de- 
signed special balsa enclosures for MLRIOO laser rangefinders which will be fixed 
to the wings surface. The laser rangefinders are communicating with the autopilot 
via two separate serial ports. These serial ports were configured by modifying as- 
signment of unused digital outputs and inputs of MP2128heli onboard processor. 
The autopilot firmware was expanded with routine of obstacle avoidance. 




Fig. 16 The testing airframe - delta wing MAV 

Actually we are preparing to perform first flights which will present the airframe 
performance and give required data for the autopilot and the obstacle avoidance 
strategy tuning. When all configuration procedures are completed, the airframe will 
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be fully functional and ready to perform autonomous flights, which verifies obstacle 
avoidance strategy in different collision scenarios. 

6 Conclusion 

Figures 9-12 present results of four different simulations which routed flight paths 
of MAV being under control of the obstacle avoidance strategy. They are clearly 
proving possibilities of the strategy and allow expecting its effectiveness in real 
experiments. In each case the MAV flight succeeded and the next waypoint 
was reached without collision with surrounding obstacles. Results presented in 
Figures 9-lOindicate also possibilities of the MAV flight in streets' canyons. This is 
the essential feature for the MAV autonomous flight in urban environment. The 
obstacle avoidance strategy meets also requirements of autonomous obstacle 
avoidance what was shown in Figures 9-10. 

In summary it can be underscored that the aim of the paper was achieved and the 
research can be continued. Basing on these results it can be predicted that the 
proposed obstacle avoidance strategy will effective and can be handled by com- 
mercial autopilots like a MP2128heli and micro aerial vehicles. The advantage of 
the algorithm would be fast response thanks to real-time signal processing. Others 
methods involving path planners or image processing introduce some delays by a 
reason of much computation time. The obstacle avoidance strategy verification will 
be the next step of our research. 
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Post-Optimal Sensitivities of Flight Trajectories 
with Respect to Selected Parameters 



C. Buskens, F. Fisch, and F. Holzapfel 



Abstract. In the paper at hand, the sensitivities of optimized flight trajectories 
with respect to selected parameters are to be investigated. Especially atmospheric 
influences like wind or deviations from the standard atmosphere shall be regarded 
since those parameters strongly influence the respective optimal trajectory. 

1 Introduction 

In many guidance or control applications, an aircraft, e.g. an UAV, ought to follow a 
given reference trajectory. Often, the reference trajectory therefore is obtained by a 
high-fidelity offline-optimization of the specific aircraft mission before the flight 
mission is conducted. Then, this reference trajectory together with an appropriate 
guidance or control law are stored onboard. For the offline-optimization, certain pa- 
rameters concerning the environmental conditions are required. Usually, the values 
of those parameters are not known exactly a priori or might even be subject to 
change during the flight mission. Consequently, the offline-computed reference tra- 
jectory may neither be valid nor optimal any more. In most cases, an online 
re-optimization of the flight-mission utilizing the high-fidelity offline-optimization 
algorithm is by far too time-consuming. One possibility would be to use a simplified 
optimization algorithm that is real-time capable and can thus be implemented on- 
board. For example, Yakimenko et al. ([8], [9]) utilize high-order polynomials as 
reference functions to describe an aircraft trajectory, thereby reducing the optimiza- 
tion problem to the determination of a few parameters. The solution of the reduced 
optimization problem can then be accomplished online. In this paper, another possi- 
bility is introduced where valid, sub-optimal reference trajectories can be computed 
in real-time onboard (see e.g. [6], [7]) without any time-consuming online re- 
optimization of the aircraft trajectory. At this, post-optimal sensitivities of the 
optimized trajectory with respect to selected parameters are calculated offline. The 
sensitivities can then be utilized to restore in real-time a valid, high-quality close- 
optimal reference trajectory for the guidance of aerial vehicles if perturbations in the 
environmental parameters occur. In this case, the tracking of the guidance trajecto- 
ries by any flight control system is facilitated since the guidance trajectories can be 
corrected with respect to changed environmental influences. 
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2 Aircraft Simulation Model 



The flight system dynamics are represented by a 3-DoF point-mass simulation 
model. In the following, the equations of motion are depicted and the proper in- 
clusion of the wind influence within the point-mass simulation model is explained. 
Furthermore, an atmospheric model is given that allows to take into account tem- 
perature deviations from the standard atmosphere. 

The position equations of motions are specified in the NED-FTame. The NED- 
Frame is a coordinate system with its origin located at the aircraft's center of grav- 
ity. The X-axis of the A^ZiD-Frame is pointing into northward, the y-axis into east- 
ward and the z-axis into downward direction. The position equations of motion are: 



^x^^ 



V^/o 






' K 



sinf^ 



(1) 



/o 



where Vk is the kinematic velocity, Xk the kinematic course angle and y^ the kine- 
matic climb angle. Assuming flight over flat, non-rotating earth, the following 
simplified translation equations of motion result: 
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m • V° • cos y^ 



(2) 



(3) 



(4) 



Here, m represents the aircraft mass and is assumed to be constant. The total sum 
of external forces E F given in the Kinematic Reference Frame K is made up of 
the gravitational force Fg , the aerodynamic force F^ and the propulsive force F^ : 



The gravitational force F^j is given by 



[n\=M,„\f°\=M, 



^0^ 




\ 



(5) 



(6) 



where g denotes the gravitational constant. The transformation matrix ^ko is giv- 
en by Eq. (18). The aerodynamic force F^ with respect to the center of gravity G 
of the aircraft is computed by the following formula: 
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(7) 



where D denotes the drag, Q the aerodynamic force in the direction of the 3'-axis 
of the Aerodynamic Frame A and L the lift. The transformation M^a is given by 
Eq. (17). The dynamic pressure ^is calculated from the air density p and the 
aerodynamic velocity V^: 
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The aerodynamic coefficients are given by the following equations, where a linear 
lift curve and a quadratic drag polar are implemented: 



Q ~ QP ' Pa.cmd 

c =c 



C a 



(9) 

(10) 
(11) 



Here, aA is the commanded angle of attack and P^ the commanded sideslip angle. 
The propulsion force Fp is assumed to be aligned with the x-axis of the aircraft's 
kinematic velocity: 



(Fp). 







(12) 



The following relationship for the thrust force T is implemented that applies to 
propeller-powered aircraft: 



r = r 



ref 
max T 7 G 
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(13) 



where T,„ax is the maximum thrust, Vref the reference speed, p,ef the reference air 
density and rip the density exponent. St is the commanded thrust lever position. For 
the computation of the air density p, the atmospheric model given by Eq. (14) is 
utilized where the air density is a function of the aircraft's downward position z 
given in the A^ZsD-Reference Frame: 
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Here, r„/ is the reference temperature, y^f the reference temperature gradient and 
R the specific gas constant. Eq. (14) is valid in the troposphere layer, i.e. it is valid 
for altitudes between -2km and 11km. The reference values involved in the at- 
mosphere model are taken from the International Standard Atmosphere DIN ISO 
2533 [5]. Temperature deviations from the norm standard atmosphere can be taken 
into account by adjusting the reference temperature r„/ accordingly: 



'ref 



T +AT 



(15) 



Furthermore, a differential equation for the aerodynamic bank angle fi^ is added to 
the simulation model: 



Ma =Pc 



(16) 



with p being the commanded roll rate. The aerodynamic force F^ acting on the air- 
craft' s center of gravity is computed in the Aerodynamic Frame A and then trans- 
formed into the Kinematic Reference Frame K with the help of the transformation 
matrix M^a between the Aerodynamic Frame A and the Kinematic Reference 
Frame K (see Eq. (7)). The transformation matrix M;,^ between the Aerodynamic 
Frame A and the Kinematic Frame K is given by: 
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where xa represents the aerodynamic course angle and y^ the aerodynamic climb 
angle. The aerodynamic course angle and the aerodynamic climb angle have to be 
restored utilizing the respective kinematic relationships, taking into account the in- 
fluence of the wind. Therefore, the aerodynamic velocity of the aircraft's center of 
gravity with its components denoted in the A^ZsD-Frame is first obtained by: 



{yA)>mo-m: 



(21) 
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Here, 'V^, represents the wind field given in the A^ZsD-Reference Frame with 
its components u„, v,„ and w^, where m„, points into the northward, v„, into the 
eastward and w„ into the downward direction. With the help of the aerodynamic 
velocity V^ , the aerodynamic course angle xa, the aerodynamic climb angle y^ 
and the absolute aerodynamic velocity Va can be restored: 
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In total, the aircraft simulation model comprises seven states that are the north- 
ward position X, the eastward position y, the downward position z, the kinematic 
velocity Vk, the kinematic course angle Xk^ the kinematic climb angle jf^ and the 
aerodynamic bank angle Ha- The command inputs are the aerodynamic angle of at- 
tack aA, the aerodynamic sideslip angle Pa, the roll rate p and the commanded 
thrust lever position dj. For the solution of the air race trajectory optimization 
problem described in paragraph 3, the commanded sideslip angle Pa has been set 
to zero and the commanded thrust lever position 8j has been set to 1 . 

3 Air Race Trajectory Optimization Problem 



The task of the air race trajectory optimization problem is to find the minimum 
possible race time tf for a given race track. The optimal control problem can be 
stated as follows: Determine the optimal control history 

(26) 



and the corresponding optimal state trajectory 
that minimize the Bolza cost functional 
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subject to the state dynamics 
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(29) 



(30) 
(31) 

(32) 

(33) 

(34) 

For the air race trajectory optimization problem, the Bolza cost functional re- 
duces to a Mayer functional since the only objective is to minimize the final time: 

J^tf (35) 

The state vector x, the control vector u and the according state dynamics were 
given in paragraph 2. The initial and final boundary conditions as well as the inte- 
rior point conditions are defined by the positions respectively the type of the vari- 
ous air race gates. While no equality path constraints are present, inequality path 
constraints arise from safety regulations or from aircraft performance limits. First 
of all, a certain ground clearance has to be respected by the pilots: 

z„i„-z(O<0 (36) 

Furthermore, the safety regulations require that an upper limit and a lower limit of 
the load factor n.g in the direction of the z-axis of the Body Fixed Reference 
Frame B is never exceeded: 

"zW-"z,max^O 

Besides the never-exceed speed Vmax given by the safety regulations, the velocity 
Va of the aircraft must not go below the stall speed Vsmii of the aircraft: 

Additional inequality path constraints are due to aircraft performance limitations 
with respect to the minimum and maximum angle of attack Ua as well as the min- 
imum and maximum roll rate p^: 
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PKMn-PKit)^0 



(39) 



(40) 



The corresponding values for the various bounds of the inequaUty path constraints 
are Usted in Table 1 . 

Table 1 Path constraints specifications 



Path constraints specifications 


Altitude 


Zmm [m] 


7.5 


load factor 


Hz.min L"J 


-2.0 


Hz.max L~J 


12.0 


Velocity 


V.UI [m/s] 


25.0 


Vmax [m/s] 


102.9 


angle of attack 


aA,m,„ [rad]/[°] 


-0.35/-20.02 


aA,max [rad]/[°] 


0.35/20.02 


roll rate 


Pk.™„ [rad/s]/[°/s] 


-7.33/-420.0 


PK.™ax [rad/s]/[°/s] 


7.33/420.0 



4 Parametric Optimal Control Problems with Control and 
State Constraints 



Problem (1) to (40) defines a parametric nonlinear control problem subject to con- 
trol and state constraints of the following form: 



Parametric control problem OCP(p): 



Minimize 
subject to 



F(x,u,p) 
x(t) 

xito) 
C(x(t),u(t),p) 



= 8(x(tf),p) 

= f{x{t),u{t),p), 

= (p(yp), y/-{x(tf),p) = 0, 

< 0, t&[to,tf]. 



(41) 



Here x{t) e R" denotes the state of a system and u{t) e R"" the control in a given 
time interval [to, tf]. Data perturbations in the system are modeled by a parameter/? 
gP-- R". The functions g-M"xP^R,fM"*'"xP-^R'', ^:P^]R", ^: R"xF— >R'', 
< r < n and OM"*"'xP — >]R are assumed to be sufficiently smooth on 
appropriate open sets. The admissible class of control functions is that of 
piecewise continuous controls. The final time fy is assumed to be free. 
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Note, that the Bolza cost functional in (28) can easily be transformed to the 
objective functional in (41) by simply introducing an additional differential 
equation for the integral term in (28) and initial value zero. 

5 Numerical Solution of Optimal Control Problems by 
Nonlinear Programming Techniques 

Direct optimization methods for solving the optimal control problem OCP{p) are 
based on a suitable discretization of problem (41). In this section we describe an 
approach where for simplicity the discussion will be restricted to Euler's method. 
It is mentioned that the sensitivity analysis that is utilized for the computation of 
the sub-optimal reference trajectories is not limited to the collocation method 
described below. Moreover, any other direct optimization method like for example 
a multiple shooting method could constitute the basis for the sensitivity analysis 
outlined in paragraph 6. 

Choose a natural number A^ and let t, g [to, f/], i = 1, ..., N, be mesh or grid 
points with 

t,=T,<...<T^_,<T^=tf. (42) 

For notational simplicity we assume that the discretization in (42) is equidistant: 

h:=^ -, T. =t„+(i-l)-h, i = l,...,N. (43) 

A^-1 . V / 

Let the vectors x g R" respectively u e R™, ; = 1 , . . . , A^, be approximations of the 
state variable x(t,), respectively control variables m(t,) at the grid points. Then 
Euler's approximation applied to the differential equation in (41) yields 

x'^'=x'+h-f(x',u',p), i = l,...,N-l. (44) 

We treat the control variables together with fy as the only optimization variables 
while the state variables are obtained recursively from the state equation (44). 
Therefore, we consider the optimization variable 

z:={u,u\...,u'''\u'',tf)eR^', N^:=m-N + l, (45) 

and compute recursively the state variables from (44), 

x' = x'(z,p):= x'(u\..,u''\tj-,p), (46) 

as functions of the control variables with initial condition x' = (p(p) given in (41). 
This leads to the following discretized optimal control problem 



Minimize 


F(z,p) 






subject to 


G,(z,p) = 0, 


i = 1,...,A^, . 






G,(z,p) < 0, 


i = N^+l,... 


.,A^,- 
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Discretized optimal control problem DOCP(p): 

Minimiz.e F(z,p) := gix'^ {z,p),p) 

subject to i^ix'^iz, p), p) = 0, (47) 

C(x'{z,p),u',p)<0, i = l,...,N. 

Let G{z) = (Giiz), ..., Ga^^ (z)) denote the collection of functions defining the 
equality and inequality constraints of (47). Then the number of optimization 
variables A^,. = mN + 1 and constraints M = kN + r results in a dense structure in the 
Hessian of the Lagrangian (49), whereas about 50% of the elements in the 
Jacobian of the constraints are zero. Setting the dimensions Ne = r, N^ = Ne + kN, 
the discretized control problem DOCP{p) defines a A^LP-problem of the form 



(48) 



Problems of the form (48) can be solved efficiently using sequential quadratic 
programming (SQP) methods; compare, e.g., the code WORHP of Biiskens and 
Gerdts. Instead of Euler's method incorporated into the relations (44) or (46) one 
can use higher order integration methods combined with a higher order control 
approximation. The convergence of solutions discretized via Euler's method to 
solutions of the continuous control problem has been proved in Malanowski, 
Biiskens and Maurer [4]. 

The Lagrangian function for problem NLP(p) is 

Liz,M,p) = Fiz,p) + M^G{z,p), //gM''^ (49) 

Let z be an optimal solution for NLP(p) with associated Lagrange multiplier /Z 
satisfying first order necessary optimality conditions. Consider the set of active 
indices defined by /„(/?) := {/ g {\, ..., N^] I Giiz, /?) = 0) and let m^ := #/„(/?). 
Denote the active constraints by G := {Gi)i^i^(p) and the multiplier corresponding 
to active constraints by JJ" G IR"'°. Let G" be the Jacobian with dimension m„y.N^. 
The following strong second order sufficient conditions are well known, cf., 
Fiacco [3]. 

Strong Second Order Sufficient Conditions 

Assume that 

a) F and G are twice continuously dijferentiable with respect to z and p, 

b) the gradients in G" are linearly independent, i.e. rank{ G" ( z , p)) = nia, 

c) strict complementarity Jl" > of the Lagrange multipliers holds, 

d) the Hessian of the Lagrangian is positive definite on Ker{ G" (z , p)). 
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v%^(z,/7,/7)v>0, V veKeriG'iz^p)), v^O. (50) 

Then z is a local minimum for NLP(p). 

A numerical check of the SSC (50) consists in evaluating the projected Hessian 
on Ker{ G" (z , p)) and in verifying that its eigenvalues are positive, cf. Biiskens 
andMaurer [1], 



6 Parametric Sensitivity Analysis 

A thorough treatment of parametric sensitivity analysis can be found in Fiacco [3], 
a compact overview on parametric sensitivity analysis for discretized optimal 
control problems is given in Biiskens and Maurer [1]. 

Solution Differentiability for NLP(p): 

Suppose that the optimal solution (zo, jUo), satisfies the strong second order 
sufficient conditions for the nominal problem NLP(po). Then for p near to po the 
unperturbed solution (zo, jUq) can be embedded into a C -family of perturbed 
optimal solutions {zip), fJ-ip)) for NLP(p) with (z(pq), fJ-ipo)) = (zo, fJ-o)- The active 
sets IJp) coincide with laipa) and hence it follows that ^ tip) = Q for i laip). The 
sensitivity differentials of the optimal solutions and Lagrange multipliers are 
given by the formula 



f 

G;(z„Po) 



dz , . 
-r(Po) 
dp 



iPo) 



^LJzo,Mo,Po) G:{z,,p,Y^''^ 



G:(Zo,Po) 



(51) 



dp 
Moreover, the sensitivity of the objective function is obtained from 

dF 

-j-iz(Po),Po) = L^iZo,Mo^Po)- (52) 

dp 

Note that the so-called Kuhn-Tucker matrix on the right hand side of (51) is 
regular since second order sufficient conditions (50) are assumed to hold. One 
possibility of evaluating the Kuhn-Tucker matrix and the incorporated Hessian L^^ 
consists in using BFGS-updates which are obtained in the process of computing 
the nominal solution. However, these BFGS-updates usually give rather inaccurate 
approximations. Hence, in our approach the Hessian Lj.^ and the Kuhn-Tucker 
matrix will be evaluated after computing the optimal solution (zo, fJ-o), which yields 
much more accurate results. 

Eq. (52) can be used in a first order Taylor expansion of the objective 
functional, 

dF 

Fizip), p) « F(z, Po) + --[Po](p - Po), (53) 

dp 
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where the notation [po] stands for all nominal arguments. The computation of 
dz/dpipo) via (51) yields an approximation for the sensitivity differentials of the 
perturbed optimal control solutions at the mesh points, i.e., for the quantities 

3m , ^ du' , ^ . , 

— (T,,Po)--— (PoX 1 = 1,.. .,N. (54) 

op dp 

Since a free terminal time tf is handled as an additional optimization variable in 
DOCP(po), its sensitivity differential dtj/dp{po) can likewise be calculated from 
equation (51). 

Eq. (54) grants also the sensitivity differentials of the state with respect to the 
parameter, if we take into account the recursive formulation (44) together with the 
relation (46). The state sensitivities dx/dp{ti ,p()) are obtained from 

op dp oz dp dp 



7 Result 

The method proposed for the real-time update of optimized aircraft trajectories is 
applicable to a large variety of aircraft trajectory problems if variations in envi- 
ronmental parameters occur. The main practical relevance of the wind example 
may be for civil airliners or other aircraft flying on prescribed routes. However, 
the air race example chosen as the underlying trajectory optimization problem is 
very challenging since moment dynamics is no longer negligible, the trajectory is 
highly curved in all dimensions, states and controls frequently hit saturations and 
the aircraft is flying at a high dynamic bandwidth for a longer period of time. By 
choosing the air race example it was intended to demonstrate the capability and 
potential of the approach assuming that showing its applicability for this example 
includes the applicability for problems with lower dynamics. 

In Fig. 1, the optimized race trajectory for the air race that took place in San 
Diego in 2009 is shown. The optimal air race trajectory has been obtained utilizing 
a direct optimization method. The colorization of the trajectory indicates the sensi- 
tivity of the optimized trajectory with respect to wind blowing along the y-axis of 
the depicted coordinate system. At this, the following measure for the sensitivity 
of the trajectory with respect to the selected parameter has been utilized: 




^ J ; \ 



dp 



dy' , . 
dp 



a 



+ 



dz , . 
dp 



(56) 



where x denotes the northward, y the eastward and z the downward position of the 
aircraft. As can be seen from Fig. 1, especially at the Quadro (i.e. at the 270° de- 
gree turn) where the aircraft pulls up the trajectory is very sensitive with respect to 
the considered wind influence. 
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Fig. 1 Sensitivity of the optimized trajectory witli respect to wind blowing along the 
y-axis of the depicted coordinate system 

8 Conclusions 

In the paper at hand, the parametric sensitivities of air race trajectories with re- 
spect to wind are investigated. Therefore, a simulation model is implemented that 
takes into account wind influences. The air race trajectory optimization problem is 
formulated and the discretization approach that transforms the optimal control 
problem into a nonlinear programming problem is introduced. For the optimal race 
trajectory, the sensitivities with respect to the selected parameters are computed. It 
has been found that the trajectory is quite sensitive with respect to wind if an 
acrobatic maneuver is flown that requires the aircraft to pull up. By the proposed 
method, offline-optimized reference trajectories utilized for the guidance of flight 
vehicles can be updated in real-time onboard the aircraft to give valid, close- 
optimal reference trajectories taking into account deviations of environmental pa- 
rameters. At this, the tracking of the guidance trajectories in the presence of per- 
turbed environmental parameters can be facilitated. 
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Simple Control Law Structure for the Control of 
Airplanes by Means of Their Engines 



Nicolas Fezans 



Abstract. In this paper a simple control law structure is presented for the control 
of airplanes using only the engines' thrust. For the design of such a propulsion 
controlled aircraft control law, the approach followed in this work is to look for 
the right level of performance in order to avoid both excessive engines activity and 
reduction of robustness properties. Another goal is to keep the control law and its 
tuning as simple as possible: for this a control law structure whose terms can easily 
be interpreted is proposed. The capability of the proposed control law to permit safe 
landing was shown by simulator tests as well as flight tests. 



1 Introduction 

One of the objectives of the research conducted at the DLR (German Aerospace 
Center) Institute of Flight Systems is to improve the safety of all types of aircraft. 
This objective takes a central part in many projects concerning human-machine in- 
teraction (e.g. situation awareness, training of pilots), detection of adverse condi- 
tions (e.g. gust, wake- vortices), fault detection and isolation, and reconfiguration of 
control laws. 

In the past, several incidents and accidents were caused by partial or total loss of 
aircraft primary control systems. Even though a total loss of aircraft primary con- 
trol systems is theoretically extremely rare, it did happen several times 11] with an 
estimate of more than 1200 casualties made in 1996 tlj. The fact that such events 
are extremely rare must be demonstrated during the certification process. Indeed, 
looking at the past as well as at accidents that happened more recently (e.g. DHL 
A300 in Baghdad in 2003), it appears clearly that the loss of primary control devices 
is generally a consequence of other failures with some complex relations between 
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them and in some cases related to maintenance errors or to external causes. The in- 
teractions between the faults and failures in such systems can be extremely complex. 
The author's point of view is that even with very serious work on safety analysis a 
significant number of elements of the fault tree of such complex systems will be 
omitted. The level of redundancy and the extensive use of computers and real tests 
in the design of modern aircraft allowed to reach very high safety standards. How- 
ever, human errors in the maintenance or the design of the aircraft can still happen 
as well as adverse weather conditions, wake vortex encounters, or terrorist actions. 
When technically and economically possible, design and implementation of emer- 
gency systems capable of dealing with a wide spectrum of improbable situations 
should be made. The propulsion-based emergency control law presented in this pa- 
per is worth being integrated in modern transport aircraft as such an emergency 
system. 

Several studies have been conducted at NASA in the 1990's to prove the feasibil- 
ity of controlling the aircraft and landing using only the engines ||T][2l[3l. It appeared 
clearly at that time that a system assisting the pilot is required and that with such 
a system probability of casualties would drastically be reduced. Implementation of 
such a system in aircraft equipped with an autothrottle would not require any new 
hardware (and thus there would be no increase of weight) and for many modern 
aircraft only software modifications would be required to include this new function- 
ality. However, more than 15 years later still no civil transport aircraft possesses 
such an emergency system. Even aircraft that were entirely designed after this tech- 
nology was demonstrated in flight are not equipped ! 

In this paper, the case of total loss of primary control devices is considered. For 
such a deteriorated airplane a control law based on engine thrust is designed with 
the approach of keeping a very simple structure and using the lowest gains possible. 
Longitudinal control is achieved using symmetrical variations of thrust and lateral 
control using asymmetrical variations. This control strategy is similar to the one 
studied at NASA in the 1990's under the name of "Propulsion Controlled Aircraft" 
(PCA). As the name PCA properly describes the content of the work presented in 
this paper, it will also be used. At the DLR Institute of Flight Systems, experiments 
on Fault-Tolerant Control (FTC) are made using the ATTAS research aircraft IH 
[5j |6l: some of them leading to the control of the ATTAS using only the engines. 
In particular, approaches based on Model Predictive Control (MPC) were tested 
with the aim of reaching very high performance by taking directly into account 
the limitations of the airplane and in particular of its actuators and engines. In the 
current work a low-gain approach is preferred. 

The paper is organized as follows: Section |2]presents the motivation for a struc- 
tured control law with a low-gain tuning. Section [3] discusses the properties of the 
aircraft with PCA control law that are required to permit safe landings. Section |4] 
presents the control law: its structure, tuning, and protections. Section |5] presents 
a summary of the results obtained during a flight test that took place in November 
2009 with the presented propulsion-based control law. 
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2 Motivation for a Structured Control Law with a Low-Gain 
Tuning 

Failures are not always extremely severe and in some cases it makes sense to try to 
integrate the deteriorated aircraft into the regular air traffic. The failures leading to 
the use of a control law based only on the engines are very severe. In such cases, an 
emergency would be declared and the real objective for the design of an emergency 
system capable of controlling the aircraft by means of the engines is to permit an 
acceptable landing. An acceptable landing for such an emergency system is, to the 
author's mind, a landing avoiding both the loss of human lifes and major injuries. 
Relatively low vertical speed and normal attitude (slightly positive pitch, small roll) 
are major criteria for such a landing. 

Besides, in such a severe failure case, quite strong differences in terms of dy- 
namical behavior between a nominal model of the airplane and the real deteriorated 
airplane can be expected. For instance, the Boeing 747 of the Japan Airlines flight 
123 had lost almost the entire vertical stabilizer and a significant part of the left 
wing was missing in the case of the Airbus A300 of DHL in Baghdad. These dif- 
ferences lead to the need for strong robustness properties of the system, either by 
intrinsic robustness of the control law or by its adaptation. The robustness analysis 
of airplanes with a propulsion-based control law is challenging due to the slow and 
highly nonlinear responses of the engines. Their dynamics result from a combina- 
tion of saturations, nonlinear dynamics and state-dependent rate-limiters. The use 
of typical robustness metrics for design purposes in such a case is very difficult. 
For this reason, it has been decided to focus in this work on the definition of a sim- 
ple control law whose components can easily been interpreted. Thus, the approach 
here is to compensate the lack of practically usable rigorous mathematical tools for 
such problems by the use of physical interpretations. Precise evaluations of the ob- 
tained robustness properties could also be made by means of Monte-Carlo methods 
but have not yet been considered in this study. The only evaluations that have been 
made until now rely on several hundreds of simulations. It should be noticed that 
stability of the controller-augmented airplane is not required as pilots will be part of 
the closed-loop. Moreover, a stability criterion does not permit to estimate whether 
a pilot is really able to reach the airport and land or not. 

However, to the best of the author's knowledge no classical handling qualities 
criterion is applicable to a propulsion controlled aircraft, therefore there is a need 
for a new evaluation of handling qualities for such an airplane. An objective of this 
research is thus to understand which design criteria are really important with pilots 
in the loop and which criteria can be withdrawn. The purpose of this paper is not to 
address this question, but to present the simple control law structure that has been 
designed in order to ease future simulator and flight-test studies that will address 
it. With an adequate tuning of this control law the pilots must be able to land suc- 
cessfully in a wide spectrum of situations. Later on, once the required performance 
criteria for such a propulsion controlled aircraft will be properly defined and under- 
stood, the design and the validation of such an emergency system will be eased. 
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A single set of parameters has been used on the entire flight domain and for 
all configurations. The only scheduling made takes place in the inner loop which 
controls the engines: the proportional gain depends on the rotational speed of the 
fan (Nl), see section |431 Such a choice would often be suboptimal, in particular if 
we want to force the closed-loop to exhibit some defined behavior. But for such a 
system the exact behavior of the controller-augmented aircraft is not very important: 
what matters is that pilots succeed in controlling it and finally succeed in landing 
the airplane. In our experiments and in the work presented here this single set of 
parameters with a low-gain tuning allowed pilots to control the aircraft and to follow 
both glideslope and localizer signals with a good precision. This shows that both 
high-gain solutions and control strategies based on reference models are not required 
for this application. 



3 Requirements 

In this section, the main requirements for successful approach and landing by means 
of a PCA system are discussed with focus on how desirable they are, how difficult it 
will be to reach them, and which trade-off between the performance criteria should 
be made. Obviously, classical handling qualities criteria are not applicable for an 
aircraft having a propulsion-based control law. 

3.1 Longitudinal Control 

The longitudinal motion is mainly composed of the phugoid mode and the short- 
period mode. The period of the phugoid is generally between 30 and 60 seconds for 
transport airplanes. The frequency of the short-period mode depends on the aircraft 
and its center of gravity location, but would typically lie between 1 .5 and 3 rad/s. 

Increasing the total thrust of the engines leads to an increase of the energy rate of 
the aircraft. To really know the effect of this additional thrust on the movement of 
the aircraft, the pitch equation as well as the aerodynamics and mass characteristics 
of the aircraft must be known. For typical configurations, a simplified reasoning 
can be expressed as follows: a constant additional total thrust AT/ ^'^jATi > 
leads to a positive variation of the flight path angle y and vice-versa, i.e. Z\ 7] > =^ 
ziy(/ — > oo) > and 47]- < ^ Z\7(f ^ oo) < 0. This makes it possible to control 
the trajectory of the aircraft in the vertical plane. 

With the typical frequencies and damping ratios of the short-period mode and of 
the phugoid as well as the typical dynamics of engines, no real challenge is expected 
in designing and tuning a control law assisting the pilots in the control of the flight 
path angle. Such a control law will basically consist of controlling the phugoid (ac- 
ceptable response time, good damping, and no static error on the flight path) while 
avoiding unnecessary excitation of the short-period mode. 
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3.2 Lateral Control 

The lateral dynamics of an aircraft are composed of: 

• tiie Dutch roll mode exhibiting a pair of complex conjugate and stable poles with 
very low damping, 

• the roll mode which is aperiodic and stable, 

• the spiral mode which is slow and quite often slightly unstable. 

As for the short-period mode, the Dutch roll mode and the roll mode are generally 
too fast to be significantly modified by means of the engines, in particular in the low- 
thrust domain that will generally be required for descent and approach. However, a 
control law based on thrust can easily modify the spiral mode in order to ease its 
control by a human pilot. For this, the coupling between yaw and roll is used: the 
pilot controls only the roll motion and the control law generates a yaw motion by 
means of asymmetric thrust allowing to get the induced roll corresponding to the 
pilot's commands. In previous studies PCA control laws were designed to follow a 
reference bank angle (pref that was provided by the pilot. During the current research 
activities several other possibilities have been investigated in the ATTAS ground 
simulator. In particular a rate-command attitude-hold and a combination of roll rate 
and bank angle commands are being tested. They are both based on the control law 
presented hereafter: the difference is the way pilots provide the references to the 
system. 

Some reasonable goals for the lateral part of the control law are: to permit enough 
maneuverability, to reduce pilot workload by damping the lateral dynamics, and to 
ensure acceptable disturbance rejection without any action of the pilot. 



4 Propulsion-Based Control Law 

This section presents the propulsion-based control law that was used for the sim- 
ulator tests and flight tests that are shown in section |5] The global structure of the 
control law is first presented (section [4711 1. After that all elements constituing this 
structure are presented separately in sections |4!21 - I4. 51 The easiness of the physical 
interpretation of each component will appear clearly. 



4.1 Global Control Structure 

The global control structure is presented in figure[T] This structure is based on a cas- 
cade control strategy with inner loops controlling the engines through commands in 
terms of Power Lever Angle (FLA) and outer loops controlling the longitudinal and 
lateral motions through symmetric and asymmetric thrust. The inner loops have a 
crossfeeding in case of saturation as detailed in section 14.51 (see signals PLAi„/L 
and PLA„„R). A block labeled "Mixing priorities & Protections" connects the outer 
loops to the inner loops by allocating longitudinal (Nlcmd) and lateral (ZiNl„„^) 
control actions to the two engines while satifying the limits for each engine. 
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Fig. 1 Global architecture of the propulsion-based control law 



This leads to the two references NIL,^/^ and NlRre/ that are provided to the inner 
loops. Although this does not appear very explicitly in figure[T]the "Mixing prior- 
ities & Protections" block also connects the two outer loops by means of the anti- 
windup feedback signals Nl^f and ZiNlja,. This particular point is more detailed in 
section l4~4l 

On one side imposing a particular structure on the controller generally restricts 
the maximum achievable performance and robustness. On the other side it allows to 
guarantee both, the physical meaning of the controller's parameters and the simplic- 
ity of the future implementation on on-board computers. In this work the trade-off 
between reachable performance, engine activity, and robustness is investigated un- 
der the assumption that a simple and comprehensible structure is required. Using 
such a structure is therefore not a choice made during this work but a constraint 
directly included in the addressed problem itself. 



4.2 Longitudinal Controller 

The longitudinal controller is presented in figure |2a| It uses a PID structure with 
a filter {K„\s)) on the derivative part and two feedforward elements: a dynamical 
one KpprJs) and a static one KfFj. The filter K„_{s) is required to remove the 
medium and high-frequencies measured by the accelerometer and therefore remove 
undesirable control activity due to gusts and turbulence. As the phugoid mode is 
very slow with respect to the typical frequency content of these types of atmospheric 
disturbances, a simple first-order low-pass filter with a bandwidth higher than the 
frequency of the phugoid mode was used. 

To get the desired reference tracking of the flight path angle 7, the static gain 
KpF rJ(O = 0) must be equal to 1 . The degrees of freedom provided by these trans- 
fer functions allow seperate tuning of the reference tracking dynamics and of the 
disturbance rejection properties. For instance, defining Kfp fyis) as a low-pass fil- 
ter permits to use a relatively high value for Kr y without getting high overshoots 
of the reference tracking response and without requiring high values of the deriva- 
tive gain. Such a tuning is interesting because it gives the pilot a control input that 
is not excessively sensible in the medium to high frequency domain while permit- 
ting quite efficient rejections of disturbances by the feedback without any action of 
the pilot. As the obtained performance was already satisfying with a simple gain 
for Kpp y, this part of the feedforward has been kept static. If finer tuning of the 
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(a) Longitudinal: y-mode 
Fig. 2 Longitudinal and lateral controllers 
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(b) Lateral: (|)-mode 



reference tracking response is required some additional degrees of freedom could 
be obtained by using a transfer function instead of the Kff y gain. 



4.3 Lateral Controller 

Tiie lateral controller is based on the same principle as the longitudinal controller 
and therefore has a similar structure. However, the types of dynamics to be con- 
trolled by these two controllers are quite different. In the longitudinal case, the static 
response to a symmetric variation of thrust is a variation of the flight path angle J, 
which is directly the variable that must be regulated. In the lateral case, the static 
response to an asymmetric variation of thrust is a variation of the sideslip angle 
/3, which is related to the second time derivative of the regulated variable ^ (bank 
angle). 

This is a logical consequence of the fact that there is no roll authority and roll is 
now controlled by means of the yaw-roll coupling. Therefore, the chosen controller 
structure (figure [2bt is not only based on the bank angle ^ and the roll rate p, but 
also on the yaw rate r. Feedback based on the sideslip angle /3 would probably also 
help getting good closed-loop properties, but /3 is generally not used in flight control 
systems for various practical reasons. This fact has been taken as a constraint and is 
not discussed. The feedback of p and r should ideally be restricted to high frequen- 
cies in order to avoid counteracting normal turns. Indeed the feedback Kca, ensures 
rejection of such side effects and simple gains Kp and Kr can be taken instead of 
some high-pass filters. 

During the landing task, the required corrections for the lateral movement are 
much more demanding than for the longitudinal motion. This leads to a more dif- 
ficult tuning of the lateral controller. The balance between good enough handling 
qualities and high activity of engines is more difficult to find, in particular when 
considering uncertainties of engines' dynamics. As mentioned in the introduction, 
the analysis tools for this type of nonlinear system are difficult to put into practice 
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and with a pilot in the loop stability is neither a sufficient nor a required condition. 
Thanks to the physical interpretations of each part of the controller, a good set of 
parameters can be found in a short time. However, a more systematic and rigorous 
methodology would be desirable. 

4.4 Mixing Priorities and Outer Loop Protections 

Authority on both longitudinal and lateral movements is severely restricted. The 
same actuators (i.e. engines) are used for both and thus some mixing priorities 
should be defined. As the controlled dynamics of the lateral motion are faster than 
the controlled dynamics of the longitudinal motion, it seems logical to give priority 
to the lateral control action over the longitudinal control actions in the case both 
cannot be satisfied simultaneously. Moreover, a poor control of the spiral mode will 
strongly disturb the control of the flight path angle, whereas the opposite coupling 
can be neglected in most cases (e.g. without stall, overspeed, etc.). This priority 
can be implemented as shown in figure |3] where the absolute value of the lateral 
control signal is used in the definition of limits applied to the longitudinal control 
signal. This permits to ensure that the sum of both control signals will respect the 
constraints of each engine and to give priority to the thrust difference between them 
over the mean thrust. 
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Fig. 3 Mixing priorities and outer-loops protections 



In figure [3j signals Nl^af and ziNl^a, are respectively the differences between 
input and output of longitudinal and lateral control saturations. They are used for 
the "integrator hold" antiwindup strategy that is implemented in both controllers 
(figures [2all2b] |. After giving priority to the thrust difference over the mean thrust, the 
signs of these signals are compared to the sign of the signals entering the integrators 
in figures |2a| and |2bl If their signs are identical, then the corresponding integrator 
would be winding and this is prevented by holding the integrator at its previous 
value. This way of preventing integrator windup does not allow to desaturate the 
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command in order to keep some authority, but in the considered application there is 
no need for a more complex antiwindup strategy. 

4.5 Inner Loops for Engines Control 

The structure of the inner loop controllers is depicted in figure |4l In this figure the 
controller of the left engine is shown: the letters L and R are use to distinguish left 
and right. For the right engine, the letters L and R must be inverted in this figure. The 
controller is basically a PI controller with a static feedforward on the proportional 
part and an "integrator hold" antiwindup strategy. In addition, the saturation signal 
of the other engine (here PLAsa,R) is crossfed in the output in accordance with the 
priorities defined in the previous section. 




PLA„„ L 



Fig. 4 Inner loop for the left engine control. Invert L and R for the right engine. 

During our tests (both in simulator and in flight), the aforementioned antiwindup 
and crossfeed in these inner loops did not appear to be absolutely required as outer 
loop gains were intentionally chosen relatively low. For more performance-driven 
tuning or in the presence of uncertainties these elements are likely to be of the 
greatest importance. As they result in negligible increases of controller complexity, 
they should be kept for all implementations of such controllers. 

Note that in our applications, the parameters of these loops were systematically 
taken equal for both engines even though significant differences of the left and right 
engines were known and can be observed at medium to high frequencies (with re- 
spect to closed-loop bandwidth). Note that engines may also differ significantly dur- 
ing their life and the control law must be able to deal with such variations. This can 
be achieved through the robustness of the control law or by means of adaptive tech- 
niques. Adaptive techniques have not been considered at all during this work. 



5 Summary of Flight Test Results 

After several simulator tests of the system, the system was tested on Nov. 20, 2009 
with the DLR ATTAS (VFW-6I4) research airplane. Goal of this flight test was to 
verify and test the handling qualities provided by this propulsion-based control law 
for navigation, ILS intercept, approach, and go-around. During previous simulator 
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tests pilots were asked to rate several possible tuning options for the control law 
parameters and to express their preferences. These preferences were also put into 
perspective with their foreseeable drawbacks in terms of robustness, disturbance re- 
jection properties, engine activity. During this analysis, rough assessments of the 
closed loop at 1 1 points of the flight domain were made. The considered points are 
based on 3 different flap configurations, 4 different speeds, 3 different altitudes, and 
landing gear extended or retracted (note that not all combinations of these parame- 
ters were considered). A 200 ms delay was introduced at engine inputs and for each 
flight condition the frequency of the Dutch roll of the ATTAS model was artificially 
shifted until the closed loop was destabilized. Besides, simulations with demand- 
ing inputs (e.g. successions of maximal stick inputs at various frequencies around 
the bandwidth of the stick to output transfer function) as well as simulations with 
initial conditions were performed with the aim of analyzing both the input-output 
behavior (including its nonlinearities) and the internal dynamics of the closed loop. 
The objective was not to compute robustness margins, but rather to obtain a qual- 
itative evaluation of each set of parameters regarding robustness and disturbance 
rejection properties. More complete and precise evaluations will be performed later 
for validation purposes and not for design purposes. This analysis lead to test two 
controller parameter sets (called tuning A and tuning B hereafter) during this flight, 
even though a clear preference for tuning A had already been identified before the 
flight. It corresponds to a faster but less damped behavior for the lateral motion. 
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Fig. 5 Reference tracking with the propulsion-based control law. 



Weather was sunny and cloudless. Wind at 2500 ft was 40 to 50 kt from 220° 
and wind on ground was 5 kt from south-east. Light turbulence. After take-off and 
activation of the experimental systems, some maneuvers were made by the test pilot 
in order to check the dynamical behavior of the aircraft with this control law and to 
provide data with strong enough inputs for future analysis. A short sequence of these 
maneuvers is shown in figure |5] It shows how well the references were followed as 
well as how the deviations caused by the very light turbulence are rejected in spite 
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of low engine activity. After that the entire sequence "navigation, ILS intercept, 
approach, and go-around" has been flown four times. 

The preference for tuning A was rapidly confirmed before attempting to land and 
therefore three of the four "intercept, approach, and go-around" sequences flown 
were performed using the tuning A. The ground tracks of the sequences are shown 
in figure |6l The longitudinal control was identical in both parameter sets and the 
glide slope could be very well followed (results are not shown here). During the 
four approaches, both localizer and glide-slope deviations could easily be controlled 
with less than a half dot precision. In figure[6l the + symbols show the point where 
the go-around was initiated and the x symbols where the flight path angle became 
positive. 

Comments of pilots and flight engineers made in the flight test report were that 
"the airplane can very well be flown with careful inputs", "desired heading can be 
exactly controlled", "during height changes maximal overshoot was 100 ft", and 
"there is no concern about pushing the experiment until landing on a long and wide 
runway in calm air and without lateral wind". Besides, during the flight and the 
post-analysis, it appeared that dynamics of the engines at very high thrust were 
significantly faster than predicted by the model used. This lead to a limit cycle in the 
lateral control (about +/— 3° of jS) loop during max climb maneuvers. After having 
analyzed the flight-test data, the correction of this limit cycle was straightforward 
by adjusting the gains on the inner loops at high Nl values. As low Nl values are 
required to descent, this gain modification has no consequence on handling qualities 
during approach and landing. 



6 Conclusion 

To conclude, a simple structure for a propulsion-based control law has been pro- 
posed. Simulator as well as flight tests have shown that performance obtained with 
this structured control law with a low-gain tuning is clearly sufficient for the pur- 
pose of landing. Further tests are required and planned in the next months which 



162 N. Fezans 

includes actual landings with this system. Some of the typical autopilot modes are 
being developed as outer loops generating the references (pref and Yref given as input 
of the control law that is presented in this paper In particular an autoland function is 
currently under development.The evaluation of the pros and cons of each mode de- 
pending on damage cases while taking into account the variability of pilot behaviors 
and weather conditions will be performed in the near future. 

This work has been applied to the VFW-614 ATTAS airplane in both flight tests 
and simulator tests. Within the next months, it will also be adapted and pursued 
using two simulators of the DLR Institute of Flight Systems: the Airbus A320 
ATRA (Advanced Technology Research Aircraft) simulator and the "Future Mil- 
itary Transport Aircraft" simulator. 
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The Development of Perspective Displays for 
Highly Precise Tracking Tasks 



Alexander Efremov and Mikhail Tjaglik 



Abstract. A generic technique is presented for optimization of the display-aircraft 
system. The technique includes mathematical modeling based on the modified 
Hess model and selection of pilot model parameters by minimization of the vari- 
ance of predicted path angle error. The prediction time is selected through mini- 
mization of variance of error characterizing the goal of the investigated piloting 
task. These tasks are refueling, terrain following and landing. The experiments 
fulfilled on a ground based simulator with Head up Display (HUD) demonstrated 
the tunnel image with preliminary selected sizes and projection of predicted path 
angle on a surface sliding in the tunnel and located on appropriately selected 
distance ahead. The results demonstrated that the developed technique allowed 
considerable improving accuracy in each piloting task. 

Symbols: 

e(t) - error: 

e(t) = Ae — i{t) — £ (t) - error in single loop compensatory task; 

e(t) - AH(t) - H — H (t) - error in landing, terrain following task; 

e(t) = A£g =egit)-H./L - error in refueling task; 

i(t), H (t) - input signals in different task; 

L - predicted distance; 

n^ =dn/da', 

n - normal acceleration; 

T =L IV - prediction time; 

pr pi- 

Z^=dZ/dW-l/m; 

W - vertical component of perturbed translation velocity of aircraft; 
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^ , CO - damping ratio, frequency in short period motion; 
Y =Yr 12 + Y - predicted path angle; 

£g = d + H/L - aiming angle; 

H - altitude; 

Y- flight path angle; 

L - distance between probe and drogue, distance between pilot and surface 
were predictor is projected; 
AZ, AH - sizes of tunnel. 

1 Introduction 

Number of piloting tasks such as terrain following, refueling, spot landing (includ- 
ing carrier landing are precise tracking tasks). All of them are characterized by the 
necessity to control the aircraft flight path. The aircraft dynamics has second and 
third order pole at the origin in these cases. It requires additional information 
(climb rate, pitch, yaw angles etc) which pilot uses to close additional feedbacks 
to ensure system stability and accuracy. To ease piloting process in some tasks 
(landing, terrain following) they use director indicator generating a director signal 
improving the dynamics of display-aircraft system. However, such director indica- 
tors do not change the compensatory type of the task. Recently a number of 
researches [1,2] has been made to develop a new type of displays capable of 
generating 3D elements (like tunnels) and additional signals with information in- 
dicating future position of aircraft. The results of investigations demonstrated that 
displays of this type allow improving accuracy considerably. Below is a generic 
approach towards the display design for some piloting tasks. 

2 Approach 

Three precise tracking piloting tasks: were considered landing, terrain following, 
refueling. When landing and terrain following the longitudinal control is charac- 
terized by tracking of program trajectory Hi(t). When landing such trajectory is a 
glide path. In the case of terrain following it might by defined preliminary on the 
basis of required proximity to the terrain. Its definition has to take into account 
limits of accelerations, path angles, climb rate and minimum altitude. The pilot 
has to minimize error e(t) between Hi(t) and aircraft altitude H(t), 

e{t) = AH = H^{t)-H{t) (1) 

in both tasks. 

The transfer function of aircraft in these tasks 

His) _ Kyv ^^^ 

SAs) s^(s^ + 2l^^o}^pS + a),/) 
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has the second order pole at the origin. It is well known that precise control in dy- 
namics of this kind is very difficult. 

As far as refueling is concerned, the controlled element element dynamics is the 
angle ("aiming error") 



where L is a distance between the pilot and the drogue (basket) (Fig 1). When re- 
fueling the pilot has to minimize the error e(t), where 



e{t) = A£g=eg(t)- 



Hi(t) 



(3) 



And Hi(t) is the basket displacement. 




Fig. 1 Kinematics of refueling 



£ (s) 
The transfer function — of controlled element dynamics is the following 

£s(s) _ K^{s^-Z"s-Z"V/L) 



SAs) s\s^+2l,pa),ps + a),/) 



(4) 



In case of short distance to the basket V/L»l this transfer function becomes 
close to 






KA-Z")V/L 



\s- +2^ 0) s + coj) 



This transfer function is similar to (2). It means that the pilot compensation in this 
task has to be considerable too. The gain coefficient in this transfer function 

( K^Z "V I L) for short distance L is considerable too. For this reason the pilot 

changes the strategy to suppress the instability in pilot-aircraft system. The pilot 

starts adjusting the aircraft altitude rather than the angle Eg [3]. Such dynamics is 

not simple one, but it is characterized by a constant gain coefficient. 

Controlled element dynamics can be improved in a number of ways. Among 
them is employment of a flight control system capable of altering parameters of 
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denominator D(s) of aircraft transfer function W 



alteration of numerator N(s) of transfer function W^ 



_ N(s) 
D(s) 

N{s) 



The other way is 



D(s) 



for any considered pi- 



loting task. It can be realized by the image of predicted path angle y (t) on HUD 

and its projection on the surface locating at a distance L^, and sliding in front of 
the plane with the velocity corresponding to the aircraft velocity. The value of 
Lpr opt (or Tp,. op,=Lpr op/V) is defined through the simulation on workstation or 
mathematical modeling by calculation of (j^ = f(T ) and definition of its mini- 
mum. The error e(t) is defined by (1) for landing and terrain following tasks or by 
(3) for refueling task. Efficiency of information on the future position of the air- 
craft was verified through the fixed-based simulation, where a computer-generated 
visual system (CGVS) created an image close to the investigated piloting task. 
Moreover, CGVS also generated HUD images of a predictor and a tunnel. By vir- 
tue of such tunnel pilot can see the aircraft position in the space and change his or 
her compensatory behavior to the preview type. The efficiency of such transfor- 
mation has been shown in a number of studies [4]. 



3 Analysis of Prediction Distance Lpr on Pilot Aircraft System 
Variables 

To optimize Lp^ or corresponding prediction time (Tpr=Lp/V) it is necessary to 
take into account its influence on the following task variables: controlled element 
dynamics WJs), transfer function Wc defining the relation between variable char- 
acterizing the task performance (H(s) in landing and terrain following, and e^is) 
in refueling) and controlled element dynamics output signal e^, input signal. 

They are defined below these variables and influence of Lpr on them for each 
investigated piloting task. To define controlled element dynamics it is necessary to 

know predicted path angle y (t) . The Fig. 2 shows that y (t) = y—^ + y , 



where Tpr=Lp/V. 




Fig. 2 Definition of predicted path angle Fig. 3 Projection of predicted path angle 
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For all piloting tasks under consideration change of altitude influences on the 
location of projection of prediction path angle yp,.(t) on surface at distance Lpr 

(fig 3). It influences controlled element dynamics output Ey. = y + H I L . 

The block-diagram of the considered task (Fig 4) allows to obtain the transfer 
function of controlled element dynamics 



W. 



Syis) 



K^{T„y+2s + 2IT„^) 



X, {s) 2s^{s^ + 24 (W 5 + co,p ) 



(5) 
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Fig. 4 Pilot-aircraft system for Ey angle control. 



In general the dynamics (5) with the constant T,,, (or L,,,) provides better flying 
qualities in comparison with the dynamics characterized by the variable parameter 

LprXor Tpi). The parameter Lpr influences on the transfer function W^ . 

Below is the transfer function in landing and terrain following tasks: 

H(s) 2V 



W. 



Eyis) 



Tp^s'-+2s + 2ITpr 



(6) 



In case of decrease of Tp,., equation (6) can be simplified as W^ 



His) 

£y(s) 



V 



s + l/T, 



pr 



which does not contain the first order pole at the origin. The increase of Tp^ leads 
to increase of the delay of altitude response in relation to e^ . 

For example, when T»\ 



W. 



V 



His) 



Egis) siTpJ2s + l) 



Deterioration of harmonization between variables H(s) and £yis) can lead to 
deterioration of tracking accuracy H(t), in spite of improvement of tracking 
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accuracy e^ . This disadvantage can be avoided by corresponding selection of pa- 
rameter Tpr and additional information on current aircraft altitude position. The 
latter can be intensively used by a pilot, which means that he or she closes addi- 
tional visual feedback loop. As an example of such information, 3-D corridor 
(tunnel) was generated on HUD during experiments carried out on a fixed based 
simulator. The sizes of such tunnel AH , AZ allow the pilot to estimate current 
displacement relatively to the program trajectory. 

For refueling task the output signal is Sgis) and W^ is the following 
For short distances to the basket and the high values T^,, equation (7) transforms to 



s{sTp^l2 + \) 

It seems that response Sg is characterized by the delay in comparison with £" 
in that case. The harmonization between eg{s) and eJs) can be achieved by 
corresponding selection of parameter T,,, and demonstration of the tunnel on HUD 
with preliminary selected boundaries AZ and A// . 

A projection of angle f„ on the surface at a distance Lp^ in front of the pilot 

causes dependence of input signal i(t) from Lp„ i{t) = H I L . The increase of Tpr 
lessens variance of input signal perceived by pilot 0-^ = - 



<yTp,f 



A high Lpr affects visual accuracy threshold. Therefore, Tp,. should be of opti- 
mum value for which the variance of altitude error A/f (1) (in landing or terrain 
following task) or error ACg (3) (refueling task) is minimum. The procedure for 
its definition is considered below. 

4 Technique of Research 

The optimum value of Tp,^ is defined by mathematical modeling and experimental 
investigations. 

4.1 Mathematical Modeling 

Mathematical modeling has been carried out in the following steps: 

1 . Modification of pilot model 

2. Determination of input signal 

3. Determination of optimal value Tpr 
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4.1.1 Modification of Pilot Model 

The structure of pilot model used in the research is shown on Fig. 5: 



169 




1 



r,s + i 



'e-^^l-^ "III 



W, 



w 

*' Ml 



w: 



Fig. 5 Modified Hess pilot model 



The models of spectral density of n^ and n^ take into account the influence 
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of the visual threshold (A) : 5- 



K' 



K\ 



where stochastic gain 



coefficients are A"^ = erf {A I a ^), Kj^ =erf{AI o^) . The transfer functions 



W^ 



T^s^+?,^^s + \ 



and w., 



COk 



s + 3gffj^j Wffnf s + ftJjvM 



(^, 



NM 



=0.1, <y, 



NM 



=12 



1/sec) characterize the pilot's adaptation to kinesthetic information and neuromus- 
cular dynamics correspondingly. The selection of pilot variable parameters 
{Kj^=a , Ti^— al P , Kn, Tn) was carried out by minimization of variance of er- 
ror (j^ . Here e(t) is the error signal predicted by pilot (see Fig. 5). The procedure 

for such parametric optimization is presented in [3.7]. This model is a modifica- 
tion of the well known Hess model [5]. The proposed modification allowed to ob- 
tain a better agreement with the experimental results in low and high frequency 
ranges in comparison with the basic model. 

4.1.2 Input Signal 

An input signal in terrain following task is a program trajectory. It is a basket motion 
in refueling task and aircraft response on atmosphere turbulence in landing task. In 
the mathematical modeling a general form of spectral density spectral 
S-. = k^ /(o/ + coff has been used. The &>, parameter is defined from the require- 
ment of agreement of this spectral density to the real input signal spectral density 
corresponding to the investigated task. The gain coefficient k is selected from condi- 
tion that variance of input signal located at distance Lp,. was erf = a*^ KLp^)^ , where 



(J, variance of input signal. 
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4.1.3 Optimum Prediction Time 

The mathematical modeling of pilot-aircraft system corresponding to each piloting 
task was fulfilled for different values Tp,. For each of them it was necessary to define 

pilot model parameters {K^, T^, ...) providing minimum (J^^ (where according to 
Fig. 5 e{t) = i{t) - £ it) = Ae ), value of this minimum, variances a^fj or a^^ and 



their normalized variances 



(^Ae^ - <^A£, '''^Af^max ■ HcrC CTa// max 



^2 /^2 



Ae, 

2 _2 ,_2 



<^&£„ = (^ 



Affl ' ^Ae0 max ■ 



o 



Ae„ max > '^e max ^r^ maximum values of 



variances calculated for all Tpr. As a result, dependence cr^^ = fiT.„) is obtained 

which demonstrated that an increase of prediction time (up to Tp^\^2 sec) leads to a 
decrease of variance of error (Jf^^ . Such result is common for all investigated pilot- 



—2 



ing tasks. The result of calculation of (7/^ for landing task on Fig. (6) shows that 
the optimum value Tpr is close to 0.7 sec. 
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Fig. 6 Optimal prediction time for landing Fig. 7 Optimal Prediction time for re fuel- 
and terrain following tasks ing task 



It was shown that influence of input signal bandwidth <w, on Tp^ is insignifi- 

cant. As for the refueling task the dependence is calculated as <^Aeg ~ f ^pr ) 
which allowed obtaining optimum value Tp^. It can be seen (Fig. 7) that optimum 
value is Tp^ = 0.4H-0.5 sec. The influence of input spectral density parameter CO- 
does not influence practically on this value either. 



4.2 Experimental Investigations 



The results of mathematical modeling were verified by experimentally on computer 
and ground based simulator. The workstation simulations were fulfilled for situation 
when pilot carried out compensatory task with aircraft dynamics corresponding 
to the equation (5) with constant Tpr. Pilot task was the minimization of error 
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e(t) = Ae = i(t) - £y(t) ■ Upon each experiment pilot and pilot-aircraft frequency re- 
sponse characteristics and variances were defined. All of them were close to the 
same characteristics calculated in mathematical modeling. The typical results of 
workstation simulation (normalized variances a^^ = crle I '^\e max = fiTpr) ^rid 

.2 ,_2 



= 0"a 



/ai 



■ f(T)) are shown on Fig. 8. 
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Fig. 8a Experimental results of computer- Fig. 8b Results of computer-aided simula- 
aided simulation modeling tion 



These results substantiated the mathematical modeling that an increase of pre- 
diction time Tp,. leads to decrease of normalized variance ct^^ and optimum value 



2 
A// 



is close to rprop,=0. 7^0.8 sec. 



of Tp,. providing the minimum of variance ct 

The efficiency of prediction information was checked through experiments on 
ground based simulator. This simulator has a computer generated visual system 
which demonstrates an image of outside world corresponding to each piloting 
task. Besides, a HUD image was projected on the screen of this visual system. 
The intention was to estimate: 

• pilot subjective Cooper-Harper rating 

• mean of error m/f,J 

• mean square error aj.h)^ 

where x is path variables ( // , Y , H) calculated experimentally. 
For terrain following task two versions of HUD were investigated: 

• HUD with information shown on fig 9 

• HUD with the same image without the tunnel. 

The desired and accepted task performances were defined by a method developed 
in [6]. The task performances were defined as requirements to aircraft path posi- 
tion to stay at least 87% and 70% correspondingly of total flight time inside the 
3D tunnel. 
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Fig. 9 Head up Display for terrain following task 






Fig. 10 Director indicator used 
for the landing task 



Totally 80 experiments were carried out. Data reduction allowed to calculate 
the variance of altitude errors H(t)=H(ti)-Hi(ti), where Hi(t) is the input signal, in 
each moment ?,: 



^V(e(f,.)-m, (/,.))' 



(yiHiti) = - 



N-\ 



; N - number of experiments, m ^ (f ) - mean of 



error in moment f,. After that average mean square error for all flight was defined. 

k 

M [(T^fj ] = y^ (J^fj (r, )/ k ', where A: is a total number of moments f,-. 



During experiments the full six-degree of freedom aircraft mathematical model 
was simulated in a real time mode. 

Experimental investigations demonstrated that any considered HUD allowed 
carrying out the task very accurately (Fig. 11). Moreover, employment of HUD 
with images of predictor and tunnel allowed decreasing the mean square error of 
altitude up to 20% in comparison with cases when only image of tunnel was gen- 
erated by HUD. 
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Fig. 11 Results of experiments (terrain fol- Fig. 12 Results of experiments (landing) 
lowing) 
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Landings were fulfilled under the following conditions: 

• Instrumental landing with display shown on Fig 10. The pilot also used 
other indicators. A computer generated visual system gave pilot an image 
corresponding to the landing task. 

• HUD display with images of predictor with 3D tunnel and sliding surface 
at distance L,,, where the predictor and 3D tunnel were demonstrated up 
to H=25m. After that pilot carried out flare using the visual information. 

• HUD display demonstrated the same information during the approach 
and flare up to the touchdown. 

During the flare the tunnel transforms in curved tunnel according to the flare 
trajectory. The trajectory of flare was defined from requirement that initial 
(when H=25 m) and touchdown climb rates corresponds to the pre-defined 
values. 




Fig. 13 HUD used for landing 



Fig. 14 HUD for refueling 



Almost 90 landings were fulfilled in ground based simulator. For each of them 
the altitude and side position during approach, touchdown climb rate, touchdown 
point coordinates were recorded. The reduction of data demonstrated that additional 
information allowed decreasing the variability of touchdown point. Employment of 
HUD with prediction information up to H=25 m allowed decreasing the normalized 
mean square error in longitudinal channel up to 23% and up to 52% for HUD with 
images of predictor and 3D tunnel up to touchdown. Additionally, the accuracy of 
landing in lateral channel increases up to 4 times. 

Besides, vertical rate at touchdown point decreases in two times. Employment 
of HUD display when approaching allowed decreasing altitude deviation in two 
times. 

Refueling task was investigated in two options: 

• without HUD display 

• with HUD shown on fig 14 

Totally about 70 experiments totally was carried out. The colors of the tunnel 
changed with the function of velocity of approach V„. Red color means the case 
with y„<0.8 m/sec, green color, 0.8> ¥„ <1.2 m/sec, yellow color, V'o>1.2 m/sec. 
These colors inform pilot about aircraft speed which allows pilot to control it. 



174 A. Efremov and M. Tjaglik 

With a display of this type it becomes possible to decrease the mean square error 
of point of contact 1.5 times and ensure green color of tunnel in all experiments. 

5 Conclusions 

The method of employment of future HUD is offered for some piloting tasks 
requiring high accuracy. The method is based on principles of the general pilot air- 
craft system theory stated in [7]. The advanced versions of HUD allowed to im- 
prove the accuracy of task fulfillment 1.5-r2.5 times and to enhance flight safety. 
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Abstract. This paper summarizes some of the joint research efforts of University of 
Minnesota, University of Sannio, Computer and Automation Research Institute (SZ- 
TAKI) and Budapest University of Technology and Economics (BME) to develop 
an Unmanned Aerial System. The future aim of the research collaboration is to de- 
velop and test advanced navigation, control and fault detection algorithms running 
over an easily accessible, customizable platform in real flights. This collaboration 
resulted in a flight research platform which can be easily configured with different 
guidance, navigation and control algorithms including an easy way of code devel- 
opment and testing. The paper focuses on the fine-tuning of the system components 
implemented by SZTAKJ and BME. The related work includes sensor calibration, 
modification of autopilot code and testing of simple PID control algorithms. The 
latest flight test results are included in the paper. 

Abbreviations and Notations 
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DAQ Data Acquisition 

EKF Extended Kalman Filter 

GPS Global Positioning System 

HIL Hardware-in-the-loop 

IAS Indicated Airspeed 

IMU Inertial Measurement Unit 

PID Proportional Integral Derivative (control) 

PPM Pulse Position Modulated (signal) 

PWM Pulse Width Modulated (signal) 

RC Radio Controlled 

SIL Software-in-the-loop 

SZTAKI Computer and Automation Research Institute, Hungarian Academy of Sciences 

UAS Unmanned Aerial System 

UAV Unmanned Aerial Vehicle 

UoM University of Minnesota, Department of Aerospace Engineering and Mechanics 

UoS University of Sannio, Department of Engineering 

a acceleration 

h altitude 

H magnetic vector 

k time index 

V vector of measured voltage values 

V aircraft indicated airspeed 
superscripts: 

a acceleration related values 

ft), COq angular rate related values 

h, H altitude or magnetic vector related values respectively 

V IAS related values 
subscripts: 

meas measured quantity 

quantity related to zero output 

1 Introduction 

Recently, FedEX CEO F. Smith told the media that they would switch their fleet to 
UAV's as soon as possible (Ref. Ul). However, civil, low-cost UAV's need seve- 
ral pre-requisites to render them viable, cost effective and regulated alternatives to 
existing resources. One key aspect of these requirements is to have reliable flight 
control systems onboard. It means that advanced and fault tolerant algorithms must 
be used to guarantee the safety of UAV missions. 

A joint research project was initiated between UoM, UoS, SZTAKI and BME to 
develop a UAS that has easily accessible and customizable development environ- 
ment which makes possible significantly shorter development cycles (see Ref. Q). 
The long-term aim of this collaboration is to develop and flight test advanced, re- 
liable and fault tolerant algorithms onboard the UAV. The collaboration originally 
started between UoM and UoS. The aim was to create an open-source project with 
COTS hardware components. SZTAKI and BME later joined in and their task was 
to test the developed UAS including all its components. This means exchange of 
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Fig. 1 UAS Ultrastick aircaft 

documentation, component lists and software codes. This is a vital task to test and 
prove whether the available open-source documentation is complete or not. During 
this work, continuous communication and bug information exchange was conducted 
and several improvements were applied from which the main results are summarized 
here. 

The outline of the paper is as follows: Sec. |2] introduces the open-source UAS, 
Sec. [3] summarizes the work related to the sensor unit. Sec. |4] presents the devel- 
opments in the autopilot code. Sec. |5] and |6] summarize the flight test results. And 
Sec.|7]draws conclusions. 



2 The Open-Source UAS with COTS Components 

The selected aircraft is the E-flite Ultrastick 25e. It is a small RC airplane but has 
enough space in the fuselage to integrate all the required hardware components (see 
Fig-[D- The aircraft has 1.27 m wingspan and about 1 m fuselage length with 2.1 kg 
take-off weight (including all additional hardware). 

The general architecture that is the onboard avionics together with the ground 
components is shown in Fig. |2l Besides the conventional RC components, the on- 
board system consists of the MPC555 microcomputer, the jaNAV sensor, a fail- 
safe multiplexer board and a wireless communication modem (see Ref. [81). The 
main steps of the code development for the control system are given in Fig. [3] The 
software-in-the-loop (SIL) test environment shown in the figure was fully imple- 
mented in Matlab. While the hardware-in-the-loop (processor-in-loop) environment 
uses a nonlinear Matlab simulation model of the aircraft that is connected to a hard- 
ware architecture similar to the one shown in Fig. [21 In case of HIL, the aircraft and 
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Fig. 3 UAV control system code development steps 



IINJW sensor are replaced by their simulation models; with all the other compo- 
nents (i. e., microcontroller, multiplexer board, RC receiver) being identical to those 
appearing in the real system. 

After introducing the main system components and explaining the steps of con- 
trol code development, we focus our attention to the various modifications during 
system testing. 
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3 Modifications Related to jiNAV Sensor 

The first, and most important component of the system is the IMU. The Crossbow 
^NAV sensor is used in the UAS. It can measure three axis accelerations, angu- 
lar rates, magnetic values, static and pilot pressures, temperature and GPS signals. 
It can convert RC PPM signal into the separate PWM signals. It is a compact, 
lightweight and cost effective device with variable output frequency and packet 
types (see Ref. Ii5il). 

3.1 Experiences with Software and Its Modifications 

Crossbow provides a software - called Micro- View - with which the sensor can 
be calibrated and the sensor data can be collected. Experience with this software 
indicates that it is unreliable as it loses connection with the sensor device if a broken 
data packet arrives, furthermore it does not always set the sensor into the desired 
mode. For this reason a handling program was written using Lab Windows CVI with 
similar but extended capabilities, particularly with respect to packet error tolerance. 

The calibration capability provided by Micro- View is not satisfactory as it 
considers only the diagonal elements of the calibration matrices for rate sensors, 
accelerometers and magnetometers; furthermore it does not take temperature de- 
pendence into consideration. However, the off-diagonal elements of these matrices 
should be taken into consideration, and it has turned out that the acceleration and rate 
sensors have significant temperature dependence. 0.85 g acceleration was measured 
at 5°C instead of l.Og (measured at 25°C). This means {0.015m/ s^)/K temperature 
dependence, therefore temperature calibration is a vital task. 

jaNAV can send out either sensor voltage data or scaled data. If one makes a 
custom calibration procedure, then it is advisable to use the former data format. But 
then neither PWM, nor GPS signals are sent out in the data packet (see Ref. Q). 
As a consequence, the scaled mode packet should be modified to include sensor 
voltages (instead of scaled values). Two other modifications were also required. 

1. one of the unused timers of the microcontroller on f^NAV was used for checking 
whether the data frequency is correct or not. 

2. a packet counter was set to make the detection of packet losses possible. 

3.2 Sensor Calibration and Data Acquisition 

The next step of the development was sensor calibration. The calibration formulae 
are as follows. For the explanation of notations see abbreviations and notations. 

a = ^"(VL„,-VS) (la) 

Y^ ^\^+K'^a (lb) 

« = ^"(V„^„.-Vo") (Ic) 

H-^^(VL„-V^) (Id) 
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h+, = h + K'^ (VL.(^+ 1) - vL,.v(^)) (le) 



n.+ i = ^V^+K^ (V^„„, (^ + 1 ) - VL«. (^) ) (If) 

Equation [T] shows that bias vector and scaling matrices were calculated for the ac- 
celeration, angular rate and magnetic vector, while for the static and pitot pressure 
a scaling factor and the formulae were derived. The unknown parameters were de- 
termined using least-squares technique. As it turned out, higher order terms were 
not required as the errors of calibrated quantities were sufficiently small. The maxi- 
mum/mean absolute errors are summarized in Table [T] 

Table 1 Maximum and mean absolute fit errors 



quantity max error mean error 

acceleration 4% 1.5% 

angular rate 3% 0.8% 

magnetic data 5% 1% 

altitude 27% 2.6% 

IAS 100% 30.3% 





n = 


yyv+\i^; K" 


= K'{tv+K'{ 


-0)0 




-K. 


V+Vq^; 




K'^tv+K'^ 
= K^tv+K^ 



As can be seen from Table[T]only the IAS errors are serious, but it is well known 
that its measurement with pitot tube is inaccurate. Therefore a better fit could not be 
achieved. 

The bias vector of the angular rate (in Eq. [TJ)) was calculated from the accel- 
eration with bias and scaling in a manner proposed by Ref. |4}. The temperature 
dependent parts of the formulae are shown in Eq.[2l It soon became evident that the 
magnetic and pressure data did not need temperature calibration. 

(2a) 
(2b) 
(2c) 

Here, ty is the output voltage of temperature sensor. The calibration formulae in 
Eq. prelate to the linear fitting on the elements of the calibration matrices and vec- 
tors in Eq. [T^-[Ill- The quality of temperature calibration can be tested comparing 
the measured vertical accelerations when the aircraft is on the ground. In this case, 
the pitch angle of aircraft is about 10° so, the vertical acceleration should be about 
cos(10°) ■ Ig = 0.9848g. In accelerometer calibration the following sign convention 
was used: +1 g is output if gravity acts along the sensor's axis. The measured and 
calibrated a^ accelerations from flight tests - in May 2009 (with 20 — 25°C air tem- 
perature), October 2009 (with 15 — 20°C air temperature) and December 2009 (with 
5 — 10°C air temperature) in Budapest Hungary - are plotted in Fig.lD The mini- 
mum and maximum differences from the required value were -0.57% and 0.73%, 
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Fig. 4 Temperature-calibrated vertical acceleration in different weather conditions 

respectively. It means that the calibrated measurement was carried out with very 
high accuracy. 

Sensor data could have been collected with the ground station software, but the 
following two problems arose: 

1 . The data speed of the uplink thread was too low. After some consideration, it was 
set to lOHz. With this setup, the data speed on the ground was about 8Hz. 

2. The unreliability of the wireless connection caused data losses (and/or erroneous 
data packets). 

For the aforementioned two problems, the system identification could not be per- 
formed from ground station data. However, system identification can be carried out 
based on the data provided onboard by jiNAV at 50Hz, as this frequency is large 
enough to cover all the important modes of a small UAV. 

For this purpose an SD card-based data acquisition (DAQ) device was developed 
by SZTAKl. The developed DAQ device is shown in Fig. |5] It collects and stores 
data on a standard SD card in text files. These files can be simply read and copied 



Fig. 5 The SD card-based 
data acquisition device (de- 
veloped by SZTAKl) 
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by any operating system. A separate program was written to convert the content of 
these files into JJ.NAV packet format. 



4 Control Code Developments 

After completing sensor calibration and DAQ device construction as development 
tasks, flight tests were carried out with the Ultrastick UAV. Firstly, the system iden- 
tification tests were performed. Then, tests with different levels of autonomy were 
flown. 

During these tests the attitude estimation extended Kalman filter (EKF) was 
somewhat inaccurate. It was because it considered the measured acceleration vector 
as the instantaneous gravity vector thereby neglecting the inertial effects. Another 
problem was the proper handling of global variables in the different mutually ex- 
clusive threads as this led to certain instabilities in the control. For more details on 
thread handling see Ref . 121 ■ 

To overcome the aforementioned problems, a new and more accurate multi-mode 
EKF was developed and tested. It uses not only acceleration and magnetic vectors, 
but relies on GPS speed measurements as well. This development is presented in 
detail in Ref. fJl. 

The problem with mutual exclusions (mutex's) to handle global variables was 
solved by associating a global variable with one thread only, and using its local 
copies in every other thread. This way, mutex's are used only for a short time (to 
ensure copying the global values into the locals) thereby minimizing the locked 
times of the threads. 

At the final stage of the development, flight tests with the modified MPC555 
code were conducted. The developed control code has two levels: the low level 
implements the roll and pitch angle tracking PID controllers, while the high level 
control code is responsible for altitude and IAS hold and waypoint tracking control 
(PI and other controllers). The waypoint tracking solution is described in Ref. ^. 
For more details see Ref. ISl. 



5 Low Level Control: Tracking of Roll and Pitch Doublet 
References 

The low level control tracks roll and/or pitch Euler angle references with PID loops. 
It enables straight and level flight or turning of the aircraft. At first, the tracking of 
roll then the tracking of pitch doublets was tested by switching the aircraft between 
tracking modes. SIL, HIL and real flight tests were carried out following the in- 
dividual development steps. Real flight results are plotted in Figs. |6]and|71 Tests 
were done on 17th June 2010 with 20-25°C air temperature and moderate wind. 
The tracking error for roll angle was about 2.5-7.5%, while for pitch angle it was 
about 3.5-6.5% 
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6 High Level Control: Altitude and IAS Hold, Waypoint 
Tracking 

After testing the low level controllers, we turned our attention to the higher level 
controllers. Four different autopilot versions were tested in SIL, HIL and real flight. 
The first one saves the instantaneous IAS and altitude at the time of switching and 
tracks them while flying straight and level (applying PI controllers). The second 
autopilot version tracks an IAS step function while holding the altitude (saved at 
the time of switching) and flying circles with constant —30° roll angle. The third 
one tracks an altitude step function while holding the IAS (saved at the time of 
switching) and flying circles with constant —30° roll angle. The fourth one is the full 
version of waypoint tracking autopilot. This latter flies between GPS coordinates 
while holds the altitude and IAS. 
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Fig. 6 Tracking of a roll doublet reference signal in real flight conditions 



The results were acceptable for all the autopilot versions. Tests were done on 
29th June 2010. with 25-30°C air temperature and high crosswind. The tracking 
results for IAS and altitude references can be seen in Fig. [8] After some transient, 
the IAS reference was held with ±2 m/s,±9%, while the altitude with +10m/ — 
15 m, +8%/ — 12% maximum error. 

The tracking of waypoints can be seen in Fig. |9l The crosses show the part of 
flight path when the aircraft was flown by the autopilot. The small circles with the 
crosses show the waypoints to be tracked, while the large circles show the tolerances 
(20 m) around the waypoints. The figure shows that the waypoints were tracked 
with good accuracy (7m and 0.97m errors) so, the crosswind was well compensated. 
Unfortunately, the flight had to be ended earlier due to the diminished battery power. 

The overall experiences show that low level controllers work well, while the high 
level controllers need to be tuned further to decrease the tracking errors. The IAS 
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Fig. 7 Tracking of a pitch doublet reference signal in real flight conditions 

tracking part (with elevator) and altitude tracking part (with throttle) has a tendency 
to work against each other. It is because they have very different bandwidths (the 
IAS tracking part is faster then the altitude tracking one) that should be tuned and 
harmonized. 

We experienced some problems concerning the reception of GPS signals. The an- 
tenna provided with jiNAV frequently loses reception which makes waypoint track- 
ing impossible. Therefore, another better antenna must be purchased to improve 
flight performance. 

IAS and altitude 
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Fig. 8 Tracking of IAS and altitude references in real ilight conditions 
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Fig. 9 Tracking of a route with four comer points in real flight conditions 



7 Conclusions 

This paper reports on some of the joint research efforts of UoM, UoS, SZTAKI and 
BME. The goal of this research was to create a UAS from COTS components with 
development tools which enable to rapidly synthesize, implement, analyze and vali- 
date a candidate controller design using iterative development cycles. Another goal 
was to use open-source philosophy. This paper described the tests performed with 
the developed UAS. This included building and testing the complete system starting 
from the documentation, component list and software codes. Several HIL tests and 
flights were performed. The corresponding data were plotted and evaluated. 

As a result, the software codes related to juNAV sensor were modified to achieve 
better functionality. The sensor unit was calibrated including temperature depen- 
dence. A suitable EKF was developed for attitude estimation to improve global con- 
troller performance. Finally, the thread handling of the multithreaded autopilot code 
was corrected and optimized to achieve a better performance. 

Flight test results show the success of the project. A UAS was developed and it 
serves as a platform to implement and test different autopilot codes. The participants 
gained hands on experience with the platform. 

The future plans include system identification, implementation and testing of ad- 
vanced tracking and fault detection and isolation control algorithms. Presently, fault 
detection and isolation is a hot topic in aerospace research and it is expected to 
remain so, in the coming years. The developed UAS can be used for developing, 
implementing and testing such control solutions. 
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Visibility Cues for Communication Aware 
Guidance in Cluttered Environments 



H. Claus Christmann and Eric N. Johnson 



Abstract. This paper presents the usage of visibility based guidance cues in order 
to find waypoints useful for maintaining communication in a multi UAV (Uninhab- 
ited Aerial Vehicle), single operator system. Based upon the overlay of visibility 
graphs (for radio communication) and Voronoi diagrams (for maximum clearance 
motion paths), the paper presents simulations of three staged methods, allowing the 
computation of waypoints suitable for establishing a potential multi-hop connec- 
tion between an operator and a primary UAV in an urban or otherwise cluttered 
environment. The methods present generic solutions for 2D planes, ensuring appli- 
cability for indoor, outdoor, and other structured environments through a potential 
interconnection of several non-coplanar 2D planes. The presented methods increase 
in computational complexity as they are capable of handling more complex sce- 
narios. However, the presented methods are overall still deemed computationally 
acceptable and present themselves as good candidates for onboard implementation 
on vehicles with limited computational power. 

1 Introduction and Motivation 

Tactical Uninhabited Aerial Systems (UAS) often utilize a single Uninhabited Aerial 
Vehicle (UAV) tele-operated by a single control station operator. Though higher 
level control, i.e. the use of preprogrammed waypoints or whole trajectories, is 
sometimes possible, the remote operators often pilot the UAV directly via a first- 
person video feed, providing them with immediate sensor data and allowing them 
to perform tasks such as obstacle detection and classification, collision avoid- 
ance, and path planning. lU These first-person video streams in combination with 
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stability augmentation systems for remote piloting allow for increased ease of opera- 
tion, high situational awareness of the operator, and direct availability of the primary 
sensor data, the video feed. All that is achievable with relatively modest training re- 
quirements - including the control station operation and the actual remote-piloting. 

However, this single-operator-single-vehicle setup limits the operational range 
of such a UAS to essentially the range of the utilized communication link. Fur- 
thermore, given, for example, the height of urban high rise buildings, position- 
ing UAVs "above and behind" (as required by Line-Of-Sight (LOS) constraints) 
might not always allow suitable sensor access to the back side of Radio Fre- 
quency (RF) obstructing objects. Using a relay can mitigate those limits. More com- 
plex UAS, e.g. current High-Altitde-Long-Endurance (HALE) or Medium-Altitude- 
Long-Endurance (MALE) systems, can utilize indirect communication via commu- 
nication relay nodes to overcome this LOS limit, most often at a cost of link delay 
and the addition of an operator dedicated to payload related activities. 

For tactical scale UAS the use of satellites as relays is prohibitive, not only due 
to the introduction of high latency, but foremost for infeasibility of the implementa- 
tion of related required avionics. Relying on potentially available HALE or MALE 
systems to act as relays is also challenging. Not only would the local tactical UAS 
operator have to coordinate with a different UAS to negotiate operational areas and 
coverage, but also would the link to the relay HALE or MALE UAV have to be 
robust to shadowing and/or multi-path effects in cluttered urban environments. 

Instead of external pseudolites, other local UAVs from within the same tactical 
UAS could be used as communication relay nodes, effectively establishing a local 
multi-hop network within the UAS. 

However, if operated under the same principle of remote-piloting, introducing 
additional UAVs as relays comes at the cost of drastically increased workload. Each 
additional relay UAV would require a similar amount of work as the primary UAV, 
mainly work related to collision avoidance and path planning. For those secondary 
UAVs, path planning is furthermore complicated by the dual task of getting from 
one location to another as well as maintaining LOS to the primary UAV and to the 
GCS or other intermediaries, respectively. 

This work proposes visibility based cues that could allow secondary UAVs to 
conduct these relay tasks without major operator intervention, combining opera- 
tional advantages of smaller scale tactical UAS with the benefits of swarm-enabling, 
higher-level automation in the background. 

1.1 Limiting the Operational Zone of the Secondary UAVs 

Starting from the HALE and MALE analogy, an initial replication of such a setup 
seems suitable. The system would deploy a single supportive UAV as a relay, this 
UAV would position itself "high and behind" any potential obstacle, Fig. |l(a)| and 
as such establish a dual-hop link. An extension to this would be the usage of two 
supportive UAVs, positioned high enough directly above the control station and the 
primary UAV, respectively, to establish a three-hop link. Fig. |l(b)| These setups 
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(a) Placing a single (b) Supportive UAVs (c) The vertical ap- (d) Utilizing a hori- 

supportive UAV high high and directly over proach could fail if zontal setup could en- 

and behind obstacles other nodes helps in going high is not an able otherwise infea- 

in cases of few obsta- denser environments, option for some rea- sible solutions, 

cles. son. 

Fig. 1 Open environments pose no special problems as LOS is essentially guaranteed. In 
the presence of RF obstacles, using a vertical 2D plane to create multi-hop links between 
the control station and the primary UAV provides for conceptually identical solutions. If the 
scenario does not allow for such a positioning, using a horizontal 2D plane can expand the 
solution space and enable previously not possible setups. 



could conceptually be called vertical, as the task involves the placement of support- 
ive UAVs in an essentially vertical plane determined by the position of the control 
station, the primary UAV, and the "up"-direction. 

However, "going up" might not always be an option. In certain scenarios the 
airspace could be closed above a given altitude or there is no LOS between a pri- 
mary UAV and the space above, potentially due to the Area of Interest (Aol) being 
in a tunnel, under a large bridge, indoors, or under ground. Fig. |l(c)| To include 
such scenarios, the solution would have to include the horizontal component of the 
environment, Fig. |l(d)| However, as the problem is still a 2D problem, the proposed 
generic processes can also solve this setup. 



Fig. 2 In complex struc- 
tured 3D environments, the 
generality of the solution 
for the 2D plane case allows 
for an intersecting of planes 
to capture the environment. 
In the displayed example 
(stylizing a staircase), one 
horizontal plane captures 
each floor (B,C,D), a single 
vertical plane in the stair- 
case joins them together 
(A). Requiring a UAV to be 
located at each plane inter- 
section allows inter-plane 
communication based on the 
processes presented for the 
generic 2D planar case. 
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In both scenarios, the vertical as well as the horizontal one, a large part of the 
problem can be captured in a, respectively, vertical or horizontal 2D plane. As a lot 
of human created environments tend to be "2.5D" - two dimensional complexes or 
mazes extruded in the "up"-direction and then stacked on top of each other - the 
operational zone of the supportive UAVs has been chosen to be limited to spaces 
representablq^ by a 2D plane. This provides a generic solution for the vertical and 
the horizontal setup and allows an extension into structured 3D environments by 
dissecting the environment into a set of mutually intersecting 2D planes, Fig.|2l 



1.2 Urban First-Responder Scenario 

To further motivate the application, an urban first-re sponder scenario is proposedo 
In this scenario, urban first-responders are assumed to have access to a tactical UAS 
to support their mission. The first-responders would be accompanied by a UAS op- 
erator that manages the UAS, gathers mission relevant information through it, and 
distributes the gathered data to the affected members of the team. 



Fig. 3 An urban first- 
responder scenario: the 
UAS operator is tasked with 
an reconnaissance type mis- 
sion on the target building 
(white border), i.e. a fire on 
the 13 floor. Relevant ob- 
stacles are also highlighted 
(black borders). (Aerial 
Image: Google) 








«a 








In the scenario a designated primary UAV would be under complete operator 
control at all times, providing the before mentioned benefits. The (additional) sec- 
ondary UAVs (acting as relay stations) would be fully autonomous. The operational 
zone of the secondary UAVs would be a horizontal 2D-plane at a predefined alti- 
tude, assuming that the UAVs can not go high enough to clear the buildings. This 
also supports the system's predictability for the operator, reducing operator work- 
load by eliminating questions like "What is it doing?" and "Why is it doing that?" 
In Fig. [3] a possible control station screen is presented. The mission target is high- 
lighted and has a white border, physical obstacles protruding the operational zone 
are also highlighted and have a black border. The task at hand would be to gather 
visual information from all sides of the target area of interest, in this case the white 
outlined target building. 



' In order to be allowable, the utilized projection has to maintain the visibility property of 
the mapped points, i.e. a simple top-down view is only permissible in the absence of larger 
hills, etc. 
For more details on the scenario and the motivation see J2- 
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The scenario assumes a priori availability of a map as this is required for the 
proposed processes. Sec. 12.11 elaborates how this fits into the overall operational 
scenario. 



2 Motion Map 

Motion planning is intricately connected to the map that is used and there is exten- 
sive research on the topics of map generation as well as motion planning with urban 
environments being a focus area not only since the DARPA Urban Challenge. In 
his dissertation |[3l, Wooden comments on Konig and Likhachev |4J by stating that 
"optimal planning is outweighed by the need for a "good" plan now." 

Looking for a "good plan now", ease of computation is a major driver in choosing 
methods and algorithms and, as mentioned in Sec. II. II limiting the operational area 
from a full 3D environment to a (vertical or horizontal) 2D plane is a big benefactor. 
Also, as the operational envelope of the (tele-operated) primary UAV is not at all 
affected through this constraint, the applicability of the proposed methods to urban, 
indoor, or otherwise structured environments is maintained, particularly if modeling 
techniques as outlined in Fig.[2]are utilized. 

Limiting other driving requirements to the most fundamental one, collision free 
motion, generalized Voronoi diagrams present themselves as an immediate candi- 
date to cover the motion aspect in 2D scenarios. 



Fig. 4 Voronoi maximum 
clearance paths through the 
environment. The graph 
has been stripped of leaf 
nodes, resulting in a purely 
cyclic graph that segments 
the environment in one 
connected cell per obstacle. 
No dead ends allow for 
easier usage by non-hover 
capable aircraft, such as 
fixed wing Micro-UAVs. 




Given a. f reel occupied classification of the environment, e.g. Fig.[3l a Voronoi di- 
agram providing maximum clearance paths through this environment can be easily 
computed. Held's VRONI f5l provides a computational efficient algorithm to gen- 
erate Voronoi paths through polygonal environments and the authors believe this 
algorithm to be suitable for UAV onboard implementation and use. 

In order to generate a basic map of permitted paths for the supportive UAVs, 
the Voronoi graph is stripped of leaf nodes, leaving a dead end free completely 
cyclic graph. Fig. |4l of presumably collision free paths. The graph also segments 
the environment into regions associated with each motion obstacle. The borders of 
the cell encloseing the Aol will later be called (Voronoi) perimeter. 



192 H.C. Christmann and E.N. Johnson 

2.1 A Priori Data 

The proposed processes conceptually fit into the guidance category, leaving nav- 
igation (and the related mapping), and control to other systems. As a result from 
that stems the requirement for a priori availability of a map of the environment, i.e. 
a f reel occupied classification. For first-responders in metro areas, these maps are 
assumed to be made available or, if not, created by the operator during ingress^ 
Furthermore, the processes assume the availability of onboard collision avoidance 
mechanisms which could report a mismatch between the a priori given map and the 
sensed environment and trigger a conflict resolution procedure to synchronize the 
map with the sensor information and react appropriately. 

As such, the underlying assumption is that the navigation and related mapping 
problem has been solved, either conventionally through a GPS corrected INS solu- 
tion or, for example, through a SLAM based approach as in Ii6j . 

3 Guidance Cues 

Assuming that the supportive UAVs are fully autonomous, the scenario poses a guid- 
ance problem: where to send the supportive UAVs to and how to get them there. The 
guidance task is to propose waypoints which are beneficial for the establishment of 
a Mobile Ad-hoc NETwork (MANET) in a cluttered environment. 

Several researchers have presented results on how to form and maintain MANETs 
with UAVs (e.g. Q [8] |9l), though one of the basic assumptions in the presented 
research is a free space assumption under which the establishment of a link between 
two nodes depends mainly on the distance of the nodesO 

Starting from an identical initial task - getting the primary UAV on the far side of 
a building - three methods to obtain guidance cues for where to position secondary 
UAVs are proposed. 

Without any relay nodes and the assumption that the control station operator is 
being stationary during an active use of the primary UAV, the operational range of 
the primary UAV is limited to an area that has a direct LOS to the control station 
and is within range of the communication equipment used. Fig. |5] shows this area 
for an arbitrary position in the environment introduced above. 

In order to complete the reconnaissance task given through the urban first- 
responder scenario, the operator at some point has to position the primary UAV on 
the far side of the target building in order to gather detailed data. Under the assump- 
tions of this work, a conventional tactical UAS would be incapable of achieving this 



Given readily available geo-referenced aerial images, the operator could use a familiarity 
with the area to quickly "click together" a polygonal 2D free/ occupied classification of 
the presumed operational area. Bounds on how close to approach these obstacles could be 
made conservative. 
* In flOl the authors have proposed the usage of UAS reference scenarios to evaluate the 
performance of MANET protocols in a free-space situation. 
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Fig. 5 Given a position of 
the control station operator 
(circle in the middle of the 
bottom half) the single-hop 
operational area is limited 
by range and LOS for the 
given position, represented 
by the lightly shaded area. 
Not all of the mission tar- 
get's faces (curved building 
in the middle of the top half, 
compare Fig.[3l are visible. 




as the communication between UAV and operator would be interrupted when the 
primary UAV leaves the direct LOS area of the GCS operator (Fig.|5]). 



3.1 Dual-Hop Scenario 

The first proposed method to obtain cues is based on intersecting visibility polygons. 
Using Obermeyer's VisiLibity ( IfTTIfTSlI ). the visibiHty polygons for the current po- 
sition of the GCS and the intended position of the primary UAV are computed based 
upon the environment data also used for the computation of the Voronoi paths. 



Fig. 6 The process to find- 
ing visibility cues for a dual- 
hop scenario. The Voronoi 
diagram and the visibility 
related computations are in- 
dependent up to the last step 
of the process. Guards is a 
general term for the seeing 
entities in VisiLibity, here 
they represent the (support- 
ive) UAVs. 
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Intersecting these two polygons and the Voronoi paths produces possible way- 
points which are reachable via translations on the Voronoi paths and also fulfill the 
requirement to have a direct LOS to the primary UAV as well as the GCS. Fig. |7(a)| 
shows the result of the process outlined in Fig. |6l If applicable, this method only 
requires one supportive UAV in addition to the tele-operate primary UAV. 

3.2 Perimeter Scenario 



There are cases in which the dual-hop process proposed in Sec. l3.1l either completely 
fails, i.e. there is no intersection of the visibility polygons and the Voronoi paths, or 
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(a) Outcome of the process: waypoints on the 
Voronoi paths inside the intersection of the 
GCS visibility polygons (shaded blue) and 
the primary UAV's visibility polygon (shaded 
green) could be used as guidance cues. 




(b) A feasible solution that provides very lit- 
tle robustness to motion of the GCS. Indica- 
tors are the slender shape of the intersection 
and the distance of the GCS to the edges of a 
secondary UAV's visibility polygon (shaded 
cyan). 



Fig. 7 Graphical representation of the dual-hop process results. The outcome of the process 
is not guaranteed to be usable. Even if the intersection of the visibility polygons and the 
Voronoi paths are non-empty, the solution might not be robust to movements of either the 
GCS or the primary UAV 

the resulting cues are not very robustO e.g. as shown in Fig. |7(b)| Adapting the 
dual-hop process (Sec. 13. II ) for a larger hop count (i.e. several supportive UAVs) as 
a main approach to counteract these disadvantages results in a computational load 
that might not be justifiable as the underlying method seems best suited for single- 
relay scenarios. 

Using the Aol Voronoi perimeter, i.e. the edges of the Voronoi graph that form 
the segment containing the Aol (see Sec.O, allows for the computation of a solution 
that is essentially independent of the positions of the GCS or the primary UAV and 
provides conceptually a different approach, presumably better suited for multiple 
relays. 



Fig. 8 The process to find- 
ing visibility cues for a 
perimeter scenario. The 
edges of the Voronoi di- 
agram that also form the 
edges of the cell containing 
the mission target form the 
Aol perimeter. 
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' For computation of visibility robustness see, for example, 1131 . 
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In the process outlined in Fig.[8l the problem of finding cues is translated into the 
well known "Art Gallery Problem": find the minimum number of guards necessary 
to observe all walls of an art gallery. 

In the adapted problem, the faces of the target building (the Aol) have to be 
completely observed and the supportice UAVs (the "guards") can only be located 
on the Voronoi paths forming the edges of the cell containing the Aol. Additionally, 
the supportive UAVs, the GCS, and the primary UAV have to be connected in the 
visibility graph. 



Fig. 9 Graphical repre- 
sentation of the perimeter 
process results: four guards 
(red outlined dots) are posi- 
tioned on the Aol Voronoi 
perimeter. The guards are 
connected in the visibility 
graph and their combined 
visibility polygon is shown 
(red outline). As long as 
the GCS (blue dot) and the 
primary UAV (green dot) 
do not leave this polygon, 
connectedness is ensured. 




Fig.|9]shows a result of this process. As long as the GCS and the primary UAV do 
not leave the combined visibility area of the guards, it is ensured that there always 
exists a multi-hop connection between the GCS and the primary UAV, using the 
(stationary) guards as relays. 



3.3 Dynamic Visibility 

The perimeter process provides cues for a static placement of secondary UAVs for 
the duration of a mission. However, the perimeter process might also lead to un- 
usable cues, whether due to a limited number of secondary UAVs or due to other 
constraints, e.g. a resulting congregation of secondary UAVs that exceeds a certain 
space-density and is hence deemed unsafe. As a next level, a process utilizing the 
fact that initially only two nodes in the scenario are actually moving could be used 
to reduce the computational load during a mission, potentially also making use of 
the a priori availability of a map. Fig.[TO]outlines this process. 

As supportive UAVs are limited to positions on the Voronoi paths, the visibility of 
these potential positions can be precomputed to save computational time during the 
mission. As the Voronoi paths essentially are a continuum of possible waypoints, a 
smart sampling has to be developed to reduce the computational effort while keep- 
ing a higher resolution where necessary. The edges of the Voronoi path could, for 
example, be sampled at an increased distance between sample points as the overall 
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Fig. 10 The process to finding visibihty cues for a dynamic scenario. Instead of recomputing 
the complete visibility graph of the complete environment, the visibility of select points on the 
Voronoi paths are precomputed (left half of the figure) and later expanded with the visibility 
information of the two moving nodes, the GCS and the primary UAV. This split in a static/pre- 
computed part and a dynamic/online-computed part fosters onboard realizability. 



Fig. 11 Using the differ- 
entiation of static versus 
dynamic nodes when com- 
puting the visibility speeds 
up the computation. How- 
ever, the computational load 
is still high and the results 
rather complex. Shown are 
some nodes of the static 
core and their respective 
visibility (red), as well as 
the visibility of the GCS 
(blue) and the primary UAV 
(green). 




distance of the edge to the Aol increases. This would lead to the highest waypoint 
line-density on the Aol perimeter edges and to lower densities towards the outer 
areas of the environment. 

The result of this process is not inherently geometrical and hence rather hard to 
visualize. Fig.[TT]shows some of the possible paths given by the adjacency matrix of 
the undirected visibility graph. The process takes the precomputed visibility static 
adjacency matrix S G R'^^'^ of the sampled Voronoi paths and extends it with the 
dynamic adjacency matrix D £ M.^^^ of nodes representing the GCS and the pri- 
mary UAV with respect to the the static part. Hence, instead of recomputing the 
complete visibility graph (with a complexity of 0{{N + 2)^)), the process computes 
the visibility polygon^ of the two dynamic nodes (GCS and primary UAV) and 



' In a staged approach of trying the proposed processes in the presented order, these can be 
reused from the dual-hop process. 
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checks which of the A^ sampled points of the Voronoi paths are inside of them. This 
gives the visibiUty of the dynamic and the precomputed nodes and the complete 

adjacency matrix A G k(^+2) >< ^^+2) ^^jj y^^ constructed as A = 

where S £ R'^^-^ is symmetric and D £ R'^''^. 

Any preferred graph algorithm can be used to find paths from the GCS to the 
primary UAV in the expanded visibility matrix. In Fig.[TT]a subset of the complete 
environment is shown. The potential positions of supportive UAVs are indicated 
by white circles. The corresponding visibility graph is plotted in red. This would 
be (part of) the static core. Dynamically computed would be the visibility of the 
control station (blue) and the primary UAV (green). As the visibility polygon for 
both has already been computed in an earlier step, the computation of the actual 
visibility is reduced to a checking which positions of the static core would be inside 
this polygon. 



4 Conclusions and Remarks 

The proposed methods to obtain guidance cues for communication aware UAVs in a 
cluttered environment are aimed at solving the problem of where to send supportive 
UAVs to establish a multi-hop communication network between a GCS and a pri- 
mary UAV. Feasibility of the methods has been tested in simulation for non-moving 
vehicles, a deployment simulation or actual flight test have not yet been conducted. 

The dual-hop process presented in Sec. 13. H is computationally easy and a very 
good candidate for onboard implementation. The perimeter process presented in 
Sec. 13.21 is computationally much more complex, however, as the results are valid 
for the complete mission (assuming the Aol stays identical) it could be performed 
a priori. The dynamic approach outlined in Sec. l3.3l presents a method to deal with 
a worst case scenario by dividing potential nodes in two sets, pre-computable static 
nodes and moving dynamic ones. As this process would only be reached if the other 
presented processes fail, previously computed data can be reused to minimize the 
computational impact. 

Although the dynamic process seems to be implementable onboard (from a com- 
putational perspective), this process also poses the most challenges in transforming 
the cues into actually selected target locations for secondary UAVs. As the result of 
the process is just an adjacency matrix (where shortest path graph algorithms can 
give cues for multi-hop waypoints), additional metrics have to be found or defined 
in order to rank the set of shortest (hop) paths between primary UAV and the GCS, 
assuming that fewer supportive UAVs are preferable. 

The proposed methods however do not yet deal with some of the immediately 
imminent challenges: as the primary UAV moves unpredictably for the automation 
(as it is tele-operated), the automation has to anticipate its motion and pre-plan for 
all possibilities. This might lead to conflicting requirements for the positioning of 
the secondary UAV(s) when the guidance has to sort out where to send them. Future 
work will have to look into the challenges resulting therefrom. 



198 H.C. Christmann and E.N. Johnson 

Though experimentation with realistic urban scenarios seems to indicate that the 
dual-hop process most often leads to usable results (where the use of some notion 
of visibility robustness can rank the cues given by the process in order to obtain 
definite waypoint for the underlying guidance), more simulation - particularly of 
the deployment - is necessary to provide usable and implementable heuristics and 
algorithms. 
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Part II 
Guidance and Control 



Adaptive Control of a High Agility Model 
Airplane in the Presence of Severe Structural 
Damage and Failures 



Stephan Baur, Travis Gibson, Anuradha Annaswamy, Leonhard Hocht, 
Thomas Bierling, and Florian Holzapfel 



Abstract. Adaptive control is a promising technology for future high-performance, 
safety-critical flight systems. By virtue of their ability to adjust control parameters as 
a function of online measurements, adaptive flight control systems offer improved 
performance and increased robustness. This paper addresses the adaptive control of 
extremely agile aircrafts in the presence of damages and failures. The FSD Ex- 
tremeStar, a modified version of the polystyrene model airplane Multiplex TwinStar 
II, is used as a platform for this purpose by offering a highly redundant set of control 
surfaces. The underlying nonlinear model, including the effect of all control inputs, 
is derived from first principles. A dynamic -inversion Pl-error controller is proposed 
as the baseline controller for a model reference adaptive tracking control. The result- 
ing performance is evaluated for aggressive maneuvers in the presence of elevator 
failures using the complete nonlinear model. 

1 Introduction 

In recent years, the interest in adaptive control for autonomous flight has been re- 
newed. Due to their ability to self- tune their parameters using on-line measure- 
ments, control in the presence of unforeseen damages and failures is enabled 
through adaptation in a highly satisfactory manner. Over the past thirty years, the 
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field of adaptive control has laid the foundation for analysis and synthesis of such 
systems [1]. Stability and robustness properties of these systems are well under- 
stood. Adaptation in single-input and multiple-input systems, continuous and dis- 
crete-time systems, time-invariant and time-varying systems, as well as of linear 
and nonlinear systems have been studied in depth. Over the past few years, several 
adaptive flight controllers have flown successfully [2], [3], [4], [5], [6], [7], [8] and 
enabled successful flight even in the presence of severe failures and damages. The 
tasks in these cases have focused on way-point tracking or landing. Very few re- 
sults are currently available where highly agile aerobatic maneuvers have been 
carried out in the presence of severe failures. This paper addresses the adaptive 
control in the presence of failures for a highly agile aircraft. 

2 The Aircraft Model 

In this section, a nonlinear model for a high agility aircraft, the FSD ExtremeStar, 
is derived from first principles. This model will be used for design and evaluation 
of adaptive controllers for performing high agile maneuvers in the presence of se- 
vere structural damage (e.g. parts of the wing or of the tail missing) or failures 
(e.g. reduced control surface efficiency, motors). The FSD ExtremeStar is a modi- 
fied version of the polystyrene model airplane Multiplex TwinStar II. The modifi- 
cations were conducted by the Akamodell Munchen on behalf of the Institute of 
Flight System Dynamics of Technische Universitat Munchen for investigating 
flight dynamics and flight control methods (e.g. control allocation, nonlinear adap- 
tive control) on an Unmanned Aerial System (UAS) with a large number of flight 
control inputs [9]. To facilitate this, the model features one aileron and one flap 
control surface on each wing, a canard as well as a horizontal tail with variable in- 
cidence, a rudder on the vertical tail, two wing-mounted propellers with variable 
vertical tilt angle and one tail-mounted propeller with variable tilt and azimuth an- 
gle (see Figure 1). Additionally, an independent control of left and right side con- 
trol devices is possible. The aircraft's actuation is therefore highly redundant as 
the flaps or ailerons could, for instance, be used for producing a rolling as well as 
a pitching moment. 



Variable Incidence 
Horizontal Tall 




Parameter 


Value 


Wing span 


1.40 m 


Canard span 


0.58 m 


Horizontal tail span 


0.42 m 


Fuselage length 


1.35 m 


Left/right motor 


2 X max. 500 W 


Left/right propeller 


CAMcarbon 11" x 6" 


Tail motor 


max. 75 W 


Tail propeller 


GWS8"x4.3" 



Fig. 1 FSD ExtremeStar with Illustration of Multiple Control Devices and Configuration 
Data 
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The required aerodynamic terms for the nonlinear model consist of three force 
{C^,C ,C^) and three moment coefficients (Ci,C^^^,C^^). They were calculated 

with the AeroTool, which was developed in cooperation between Bauhaus Luft- 
fahrt and the Institute of Flight System Dynamics. This tool allows the calculation 
of aerodynamics for a composition of lifting surfaces (using potential flow theory 
[10]), rotors (using momentum theory [11], [12], [13], [14], [15]) and bodies (using 
DATCOM methods [16]) by considering the influence of these objects on each 
other [17] as well as for the linear and nonlinear angle of attack range. It enables 
the user to calculate the re- 
quired aerodynamic coeffi- 
cients as a function of the 
chosen six state variables 
(a,y?,V,p,q,r) and the 16 

control inputs (9 control 
surfaces, 3 thrust vectors 
and the according tilt an- 
gles as well as the azimuth 
angle of the tail mounted 
motor). The aerodynamics 
were calculated for an angle of attack ranging from -27.5° to 27.5°, for an angle of 
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Fig. 2 Comparison of Original and Composed Model 



sideslip from -15 to 15° and the possible control surface inputs. The results for C^ 

for different motor rotation speeds are shown in Figure 2 [18]. 

Due to the 16 control surfaces and the considered six aerodynamic state vari- 
ables it is computationally intensive to carry out a full factorial design where each 
input dimension is discretized on a selected range into a number of breakpoints 
and the output is calculated for every combination of these input values. There- 
fore, a simplification of the data structure, which optimally preserves the charac- 
teristics of the original numerical model, was found. The symmetry of the FSD 
ExtremeStar to the xy — plane was used and thus only the forces and moments for 

the right side were calculated. The aerodynamics of the whole aircraft were 
determined by composing the calculated aerodynamic data for the symmetrical 
parts and by adding the calculated data for the non-symmetric components, such 
as fuselage and vertical tail [18], [21]. 

The influence of different variables on each other (e.g. angle of sideslip on the 
effect of an elevator deflection) was assessed and, based on this analysis, the in- 
fluences of second or lower order variables (e.g. influence of canard deflection on 
elevator efficiency) were neglected in the calculation [18]. 

The described simplifications helped to reduce the computation time by 
orders of magnitude by still preserving the essential dependencies of the different 
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control inputs and the overall nonlinear aerodynamic behavior of the aircraft. The 
simplifications were validated by comparing the results of a full aerodynamic 
model with those of the symmetric reduced model for arbitrarily chosen input 
variables for all six coefficients. The comparison of the obtained data showed very 
good agreement, as can be seen in Figure 2. 

The actuation of the control surfaces of the FSD ExtremeStar is performed with 
servos, which were modeled in Matlab/Simulink using a second order lag model. 
The different parameters of the model as the damping, the eigenfrequency and the 
limits for the maximum moment, velocity and the maximum amplitude were either 
set according to the information as provided by the datasheets of the manufacturers 
(e.g. maximum moment) or determined experimentally (e.g. maximum deflection). 

The FSD ExtremeStar offers three different thrust vectors, which are located on 
the left and the right side of the main wing and at the tail of the fuselage. For the 
modeling of the motor system the dynamics of the battery, of the cables and of the 
brushless controller are neglected and modeled as a resistance. The FSD Extreme- 
Star, as a high agility model airplane performing acrobatic maneuvers, will 
operate at nearly full motor rotation speed for most of the time, and the brushless 
motors are therefore modeled for simplification with a full load motor model. The 
forces and moments of the propeller were calculated with the previously men- 
tioned AeroTool, whereas the moments of inertia of the rotating parts were calcu- 
lated with an appropriate CatiaVS model. 

There are different types of sensors used in the aircraft such as an INS, dynamic 
pressure as well as magnetometer sensors, etc. For a test of the controller in pres- 
ence of uncertainties like sensor errors and bias, it is necessary to simulate the real 
behaviour of the sensors as accurately as possible. Therefore, using the data pro- 
vided by the manufacturer, models for each of the sensors were established, taking 
into account null shift bias, white noise, bias short-term stability, scale factor ab- 
solute value uncertainties and misalignment of the sensor axes [21]. 

The equations of motion for the FSD ExtremeStar are derived for a flat and 
non-rotating Earth, as the UAS only performs local missions at a low operation 
altitude. The 6-DOF equations of motion require the knowledge of the inertia ten- 
sor / [A;gm^] relative to the reference point R . The Inertia tensor was determined 

with appropriate CatiaVS models for the damaged as well as the undamaged FSD 
ExtremeStar and stored in appropriate data tables [19]. CatiaVS provides the 
moments of Inertia for the center of gravity (CoG) of the considered body and 
they therefore have to be transformed to the according reference point R . 
The dynamics of the FSD ExtremeStar consist of the states 

x^[p,q,r,q^^,q^,q^,q^,V,r,zY W 
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where the first three entries are the rotation rates, the following four are the com- 
ponents of the quaternion attitude vector and the last three are the kinematic state 
variables. The inputs are given as 



u = \jl„^ , V„ ,i,i, S^ , S^ ,S^,(T„ S^ , a , 77^,^ , 77^,^ , ^J 



(2) 



with the first two entries denoting the canard deflection, followed by the left and 
right aileron deflection, the flap deflections, the motor tilt angle and the motor ro- 
tation speed of the left and the right motor, and finally the elevator and the rudder 
deflections. 

The resulting model is given with the well-known time derivatives of: 
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(5) 



It can clearly be seen that the given model is nonlinear and provides the starting 
point for the dynamic inversion based controller [20], [21]. In this model, as well 
as in the current work, the rear motor is neglected and is a topic for future re- 
search. It can be seen that Quaternions are used for the simulation of the aircraft 
and will also be used for control design as they do not suffer from singularities 
like Euler angles. 



3 Adaptive Control of the FSD ExtremeStar 

In the following, an adaptive controller, which is based on the nonlinear model de- 
scribed in (3), (4) and (5), will be designed to accommodate for failures in the 
control surfaces. This controller is integrated with a dynamic inversion based 
baseline controller with the latter designed to ensure that the aircraft performs in a 
desired manner in the absence of any damages or failures. Given the large number 
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of control inputs, two different control allocation methods are possible. In the first 
case, the redundant control surfaces of the FSD ExtremeStar are fully used, 
whereas, in the second case, the control inputs are chosen as in a conventional air- 
craft (only ailerons, elevators and rudder) with the remaining surfaces being fixed 
at their neutral positions. In the following the attention will be focused, due to 
space limitations, on the second method. 

3.1 Dynamic Inversion Baseline Controller 

A relative degree one dynamic inversion controller, based on Quaternions, for rate 
command tracking and attitude hold was designed as the baseline controller. 

The equations and the basic principle of the implemented dynamic inversion 
Pl-error controller can be seen in Figure 3 and 4. 
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Fig. 3 Basic Layout of RD 1 Dynamic Inversion 



The pseudo control input for the dynamic inversion controller is given by: 

v = v+v =v+K {(a -(a)+K [(ca -(a)dt (6) 

It error li f.iu ^ lai ' I .aj ^ ^ im ' ^ ' 

where v^ denotes the output of the Pl-error controller with the proportional gain 
matrix K„ and the integral gain matrix K, and v is calculated based on 
the commanded m and the reference rotation rates ro by a first order linear 



reference model according to [21], [22]: 

v=(b=K((a -co) 



(7) 



with K^^^ specifying the dynamics of (7). 

The control allocation of the FSD ExtremeStar uses a constrained minimiza- 
tion, with Equation (8) serving as the cost function, where the control surface de- 
flection being necessary for a desired moment is minimized [21]. 



/ = - Au' WAu -I- yJ ( BAu - AM^^, ) 



(8) 
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where AM^^^ is the desired moment increment as provided by the dynamic inver- 
sion, and based on the current state variables and moments of the airplane, and 
Au is the corresponding incremental control input being necessary to achieve 

AM,,,. 

The weighting matrix W allows prioritizing or neglecting specific control sur- 
faces. Therefore, it allows making use of the full redundant control surfaces of the 
FSD ExtremeStar or to treat it like a conventional aircraft by only using the ailer- 
ons, elevators and the rudder. 

The constraint minimization is performed online for each simulation step 
respectively for each sampling/calculation of the real controller. This allows ob- 
taining an optimal control surface deflection for a desired commanded rotation 
rate and attitude. The dynamic inversion as well as the control allocation assumes 
an undamaged aircraft since, in the case of failures, no information about the type 
of severe structural damage or failures are available to the baseline controller. 
Therefore, the baseline controller will be augmented by an adaptive part to 
account for this. 

3. 2 MRA C Architecture for Adaptive Tracking 

By applying the dynamic inversion controller to the plant, assuming the aircraft 
dynamics are perfectly known, the overall closed loop system has a linear behav- 
iour [20],[21]. 

1 

y = -v (9) 

s 
where y is the output of the system and v is the pseudo command input from (6). 
For simplification only a single input single output (SISO) system will be consid- 
ered in the following. The input for the SISO system is the reference pitch rate 

^cMD ouier ' ^^ Commanded to the reference model, and the output is the pitch rate 

q^^^ , as achieved by the feedback linearized plant. For the proposed MRAC ar- 
chitecture an explicit direct MRAC tracking approach as shown in Figure 4 will be 
chosen. 




Fig. 4 Adaptive Control Architecture for Tracking in the Pitch Axis 
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The time derivative of the pitch rate is given with (3) to (5): 

^„«.= + -•M(x,m) (10) 



h{x) a(x)H=aB(x)H,,,^„„.„„ 

p and r can be assumed to be approximately zero, since these axes are stabilized 

by according controllers. Thus ^(x) vanishes and Equation (10) can be 
written as: 

<Ms = «(x) • M = a Bix) ■ M^^^^„.^„ (11) 

, , 9M(x) 
where B [x) ■ u^^^^^.^^ = 7]^ for the left and the right elevator being 

aM(x) 

deflected symmetrical. ?] ^ denotes the elevator deflection and the 

pitching moment due to an elevator deflection. 

In the presence of damage we can represent the corresponding pitching moment 

dM{x) 
derivative due to an elevator deflection as A , where A denotes the re- 

maining elevator efficiency with < A. < 1 and X =\ specifies the undamaged 

case. The corresponding damaged plant dynamics for the pitch axis is thus 
given by: 

%^.=^-'^g-B{^)-^dejUcncn (12) 

Since the dynamic inversion controller still assumes an undamaged plant the same 
control input, as without damage, is used and given by: 

".<.»,„ =(5(x)r-«"'-v, (13) 

It can be shown that the plant with the applied dynamic inversion results in: 

^«a. =^,-^, (14) 

For the considered case and, if the roll and the yaw rate are kept to zero, the output 
^MHs 1^ given in the frequency domain by: 

^ 

^«™= Vq (15) 

s 

The Pi-Error controller, as presented in (6), can be rewritten for each of the three 
aircraft axis (roll i = p , pitch ( = q and yaw i = r) as: 
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(k.+J.i}.)s + k.i}. 
v, =— '-^ '— (16) 

with the fixed gains k-, i}- and J- = l/A. , where (l-A) is the failure of the 

single axis. In the considered case, since only the elevator is facing reduced 

efficiency, we can chose J = J ^ =1 and J = l/A . 

The control input for each of the single axis can be written, using (16) and con- 
sidering the first order reference model for the rates, as given in (7), as: 

^,-Ji{y.,+^i^.) + k,{e,+t^,le,dt) (17) 

with the error e. ={co -co I . 

' \ 'km 'MEAS I 

While (18) can be implemented for the roll and yaw axis, this is not possible for 
the pitch axis, as 7 is unknown. 

Denoting e„ = ( V^, + z^„e^, ) and e„ = I e„ + z^^, edt 1 an adaptive control 
input is generated for the pitch axis as: 

Vc,=K^t) + k^-e^^{t) (18) 

It can be shown that V in Equation (19) is a Lyapunov function: 

1 



2 



f 1 ^ 

V 7 J 



(19) 



with J = J - J . Since k = — ( -/ ) e„ (f ) k ^e ^ (t) the time derivative of V 

is given by: 

1 -^ 
y = -^/,, (0 -Je^, (Oe,^ (0 +-JJ (20) 



It can be seen that the first part of (20) will always be smaller than zero. The sec- 
ond part will be designed in a way that it is always equal to zero and yields the 

adaptive update law. If / , the adaptive parameter in (18), is adjusted as: 

J--r\(tKJt) (21) 

the stability for the chosen adaptive law is shown [1],[21],[23] while the learning 
rate of the adaptive law can be adjusted with the constant parameter /. 
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4 Validation of Adaptive Controller and Conclusion 

In the following the derived adaptive law is assessed for the control of the FSD 
ExtremeStar with several aggressive pitch rate commands of 60 [deg/s], where 
each is commanded for a duration of 6 seconds, and with an elevator efficiency of 

A =0.2. 

(/ 

Our assumption here is that only the pitch-axis loop requires adaption as the 
failure is in the elevator. A non-adaptive PI controller, as given in (16), is used for 
the roll and the yaw axis, while the adaptive controller given by (18) and (21) will 
be used for the pitch-axis. The assumption is that the fixed controllers will ensure 
that p as well as r remain near zero and the adaptive controller focuses on the 

damaged pitch-axis. 

It can be seen that over the first few cycles, both the Pl-error controller without 
adaption (cyan) as well as with adaption (red), exhibit similar performance. 
However as time proceeds, the adaptive controller adjusts its parameters and due 
to its learned behaviour it results in a much better performance than the non- 
adaptive Pl-controller. Moreover it is also able to reestablish the desired nominal 
performance. 






= =|==t^= = = = tJ= = ==ir^ 



Fig. 5 Implemented Design for Adaptive Tracking 

The results show that adaptive control is a promising approach for the control 
of high agility aircraft in the presence of severe structural damage and failures. 
This approach will be extended and analysed in depth in subsequent publications. 
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Adaptive Control of Non-minimum Phase 
Modal Systems Using Residual Mode Filters: 
Parti 



Mark J. Balas and Susan A. Frost 



Abstract. Many dynamic systems containing a large number of modes can benefit 
from adaptive control tecliniques, which are well suited to applications that have 
unknown parameters and poorly known operating conditions. In this paper, we 
focus on a direct adaptive control approach that has been extended to handle adap- 
tive rejection of persistent disturbances. We extend this adaptive control theory to 
accommodate problematic modal subsystems of a plant that inhibit the adaptive 
controller by causing the open-loop plant to be non-minimum phase. We will 
modify the adaptive controller with a Residual Mode Filter (RMF) to compensate 
for problematic modal subsystems, thereby allowing the system to satisfy the re- 
quirements for the adaptive controller to have guaranteed convergence and 
bounded gains. This paper will be divided into two parts. Here in Part I we will 
review the basic adaptive control approach and introduce the primary ideas. In 
Part II, we will present the RMF methodology and complete the proofs of all our 
results. Also, we will apply the above theoretical results to a simple flexible struc- 
ture example to illustrate the behavior with and without the residual mode filter. 

1 Introduction 

Applications of control theory to flexible aerospace structures have been many 
and varied. The survey [13] provides a foundation for structure control with many 
control approaches and examples. This was based upon a distributed parameter 
approach to control of flexible structures and other very large-scale systems [14]. 
Later work created the idea of a Residual Mode Filter (RMF) to offset the destabi- 
lizing effect of unmodeled modes in a feedback control environment [15]-[17]. 
This RMF-based structure control theory has been applied to the complex control 
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issues for large horizontal-axis utility- sized wind turbines [18]-[21], and is begin- 
ning to be applied to aeronautic problems that currently use notch filters, eg for 
flutter, also we are applying the theory to aircraft control where there are flexible 
modes in the pilot bandwidth, e.g. large civil tilt rotor. 

In this paper, we extend our adaptive control theory [l]-[4], [7] to accommo- 
date modal subsystems of a plant that inhibit the adaptive controller, in particular 
those residual modes that interfere with the almost strict positive real condition. 
The systems we consider will be large dimensioned, linear time invariant ones 
which can be diagonalized or placed into modal form. This will include linear 
flexible structures of many types. Our adaptive Control approach allows for large 
dimensioned systems through a foundational use of Ideal Trajectories so that the 
adaptive controller is of much lower dimension than the plant. 

The modification will use the idea of Residual Mode Filters (RMF) introduced 
for fixed gain controllers in [6]. In this paper the RMF will be used to eliminate 
the effect of modes that prevent the almost strict positive realness of the overall 
system by being non-minimum phase. This is a new use of the RMF idea; in pre- 
vious non-adaptive work the purpose of the RMF was to eliminate or mitigate the 
destabilizing effect of modes unmodeled in the control system design, whereas 
here the RMF is applied to reinstate the minimum phase nature of the plant under 
adaptive control. 

Here in Part I we will review the basic adaptive control approach and introduce 
the primary ideas. In Part II, we will present the RMF methodology and complete 
the proofs of all our results using results from [8]. Also, we will apply the above 
theoretical results to a simple flexible structure example to illustrate the behavior 
with and without the residual mode filter. 



2 Rejection of Persistent Disturbances 

The Plant used in this theory section of the paper will be modeled by the linear, 
time-invariant, finite-dimensional system: 

|x^=Ax^+Bu^-Hruo 

where the plant state x (/) , is an Np-dimensional vector, the control input vector, 

u„{t), is M-dimensional, and the sensor output vector, y „(0 , is P-dimensional. 

The disturbance input vector, u^ (?) , is Mo-dimensional and will be thought to 
come from the Disturbance Generator: 

\ D D ^2) 

Zfl =Fzo;Zo(0) = Zo 

where the disturbance state, Zp{t), is No-dimensional. All matrices in (l)-(2) 
will have the appropriate compatible dimensions. Such descriptions of persistent 
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disturbances were first used in [5] to describe signals of known form but unknown 
amplitude. Equation (2) can be rewritten as in [3] in a form that is not a dynamical 
system, which is sometimes easier to use: 

" " (3) 

Zd =L(Z>o 

where (^£, is a vector composed of the known basis functions for the solution of 

U£, = z^, , i.e., ^^ are the basis functions which make up the known form of the 

disturbance, and L is a matrix of dimension N^ x dim( ^^ ). For the analysis per- 
formed in this paper, the amplitude of the disturbance does not need to be known, 
so(L,0) can be unknown. For a better understanding of the disturbance genera- 
tor, consider the example of a disturbance generator for a step disturbance; in the 
form of equation (2), a step disturbance would have = 1 and F = , in the form 
of equation (3), a step disturbance would have (?>£,= 1 . 

In [5]-[6], as with much of the control literature, it is assumed that the plant and 
disturbance generator parameter matrices, (A,B,C,r,0,F) , are known. This 
knowledge of the plant and its disturbance generator allows the Separation Princi- 
ple of Linear Control Theory to be invoked to arrive at a State-Estimator based, 
linear controller which can suppress the persistent disturbances via feedback. In 
this paper, we will not assume that the plant and disturbance generator parameter 
matrices, (A,B,C,r,0), are known. But, we will assume that the disturbance 

generator parameter from (2), F, is known, i.e., the form of the disturbance func- 
tions is known. In many cases, knowledge of F is not a severe restriction, since the 
disturbance function is often of known form but unknown amplitude. 

Our control objective will be to cause the output of the plant, y „(?) , to asymp- 
totically track the output of a known reference model, y^CO- The Reference 
Model is given by 

(4) 




where the reference model state, x,,, (/) , is an Nn,-dimensional vector. The refer- 
ence model output, y„(t), must have the same dimension as the plant output, 
y „(/) . The excitation of the reference model is accomplished via the vector, 
u„ (/) , which is generated by 

a„,=F„,u„,; u„(0)=u;r (5) 

It is assumed that the reference model is stable and the model parameters, 
(A„ , B„ , C„, , F„, ) , are known. 
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As in [5]-[6], we define the Ideal Trajectories for tiie plant given by (1) as lin- 
ear combinations of tiie plant states, the control inputs, and the disturbance inputs: 



Jx* -SiiX„ +Si2U„ +SyjZj) 
[U« =S2iX,„ +^22^111 +^23^0 

where x«(f) i&ihe ideal trajectory, u*(0 is the ideal control, u«(f) and 



(6) 



Xj, = Axj + Bu* + Fu^, ; x» (0) = x, 

y*=Cx, =y„ 







(7) 



Note that the ideal output, y*(?) , matches the reference model output, y„{t) . If 

such ideal trajectories exist, they will produce exact output tracking. 

By substituting the ideal trajectories given in (6) into (7) and by using the dis- 
turbance generator given by (2), the ideal trajectories can be made to match the 
reference model (4)-(5) with the following Model Matching Conditions: 



ASii+BS;i=SiiA„ 



ASi2+BS22 -SiiB„+Si2F„ 
ASi3-i-BS23+r0 = S;3F 



CSi 

cs: 







(8) 



csr, = 



The model matching conditions given in (8) are necessary and sufficient condi- 
tions for the existence of ideal trajectories. Solutions to these matching conditions 
must exist for later analysis, but explicit solutions need never be known for the 
adaptive controller design. Necessary and sufficient conditions for the existence 
and uniqueness of solutions to (8) are given in [9]. We repeat this result here for 
completeness and the proof is given in the Appendix found in Part II. 

Lemma 1; If CB is nonsingular, there exist unique solutions to the Linear 

Matching Conditions (8) when T{s) = C(sl — Ay B shares no transmission zeros 
with the eigenvalues o/ A^ , /^„ , or F . 

The desired control objective is for the output of the plant to asymptotically 
track the output of the reference model. We define the output error vector as: 



^ y y p J m 

To achieve the desired control objective, we want ty- 
state tracking error as follows: 



(9) 



-^0 . We define the 



(10) 
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Using (7) and (10), we can write the output error vector as: 

Cy =y p-y,n =y p-y-" =Cx^-Cx* =Ce* (ii) 

Furthermore, if we let Au — U — U« , from (1) and (7) we have 

e, =Ae, +BAu (12) 

For analysis purposes, we define a Fixed Gain Controller 

u^=u*+G*e^ (13) 

If we use the fixed gain control law (13) in the plant given by (1), combined with 
the definition of X* from (7) and the output error vector in the form of equation 
(11), we obtain: 

e, =(A + BG*c)e, (14) 

We can summarize the above by the following: 

Theorem 1. If (A,B,C) is output feedback stabilizable with a gain Gg, i.e., the 
eigenvalues of A^^ = A + BG^C are all to the left of the jco-axis, then the fixed 
gain controller, (13), will produce asymptotic output tracking, i.e., e >0 . 

If all the plant parameters, (A,B,C,r,0,F) , are known, then the fixed gain 
controller given by (13) with a state estimator for z^ would be adequate for 
asymptotic tracking. Note that output feedback stabilization of (A,B,C) can be 
accomplished when 

M +P + No>Nj, (15) 

and (A,B,C) is controllable and observable [9]. In (13), detailed knowledge of 
the parameter matrices is not required, suggesting that an adaptive control scheme 
might be possible under our original assumptions that (A,B,C,r,0) are un- 
known and F from (2) is known. 

Consider the plant given by (1) with the disturbance generator given by (3). 
Our control objective for this system will be accomplished by an Adaptive Control 
Law of the form: 

U/, = G„x,„ + G„u,„ + G.Cy. + Gd<I>d (16) 

where G„,G„,Gg,andG£, are matrices of the appropriate compatible dimen- 
sions, whose definitions will be given later. We develop the gain adaptation laws 
to make asymptotic output tracking possible by first forming the following which 
are intended to simplify our notation: 
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AG„=G„ 
AG„=G„ 
AG, = G, 



'22 



'21 



(17) 



AG^, =Gjj -S23L 



The starred gains in (17) are for analysis and come from the ideal trajectory, x« , 
of equation (6) with z^ in the form given in (3), which is then substituted into the 
fixed gain controller (13). Using (6), (7), and the adaptive control law (16), we can 
define: 



Au = u^ -Uf 



-. AG„u„ + AG„,x„, + (aG, + G:)e, + AG, 



(18) 



Then, via (11), (12), and (18), with appropriate definitions, we have: 

e* = Ae* + BAu 

= (a + BG:cK+b[aG„ AG,„ AG, AG^];/ 
= Ace*+BAG;7 



(19) 



where T] ={a^ x,jj e (j)jj] is the vector of available information. We combine 
(12) and (19) to obtain the Tracking Error System: 



e* = A^e* +BAG;7 

Now we specify the Adaptive Gain Laws: 

G = -eyH 



(20) 



(21) 



where H = [h„] , i=l,2,...,4 is an arbitrary, positive definite matrix (i.e., H > 0). 
This yields 



(22) 



Our Adaptive Controller is specified by (16) with the above adaptive gain laws 
(22). Note that none of the starred gains used in the earlier analysis appear in 
the realizable control law, (16) and (22). Next we will analyze the stability of this 
controller. 



G„ = 


-eyUmhu 


G™ = 


-e,,x„h22 


G.= 


-eyeyhas 


Gd = 


-ey«^Dh44 
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The closed-loop adaptive system consists of (l)-(5), (9), (16), and (22). Using 
(20) and (21), we have 



e* = A^e* + BAG 77 
AG = G = -ey;7^H 



(23) 



y 



Ce* 



where Ap=A + BG^C. We are able to obtain (23) from (21) because 

AG = G-G* where G*=[S22 S21 G^ S23L] is constant (although generally 
unknown). The stability of the nonlinear system (23) can be analyzed using 
Lyapunov Theory. We form the positive definite functions: 



' 2 

y2(AG) = -/race(AGH"'AG^) 



where P > is the solution of the following pair of equations: 



I PB = C^ 



-Q;Q>0 



(24) 



(25) 



These equations are usually known as the Kalman-Yacubovic Conditions. The ex- 
istence of a symmetric positive definite solution of (25) is known to be equivalent 
to the following condition: 



Tc(s) = C(iI-Ac) 'b is strictly positive real (^P/?) 



(26) 



For a proof of this equivalence, see [12] App. B. The strict positive realness of 
Tp (s) means that for some (T > and for all CO real. 



ReTc(-cr+7<y)>0 



(27) 



If we calculate the derivatives, V^ , along the trajectories of (23), we have, using 
(25), that 



Vi =--erQe* +elFBAGr] 

= --erQe,+e^v 
v = AGtj 



(28) 



and 
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V2=frace(AGH"'AG^) 

= frace(-e,,;7^H)(H"'AG^) (29) 

= —trace{e ,,v ) = —trace(e v) = — e v 

1 T 

We can form y = V, + Vj ^> V = — e* Qe* with V <0 . Consequently, 

Lyapunov theory guarantees stabiHty of the zero equilibrium point of (23) and all 

trajectories of (24) will remain bounded. This guarantees that both 6* and AG 

are bounded. 

We can summarize the above by the following Closed-Loop Stability Result: 

Theorem 2. Suppose the following are true: 

(1) All u^(/) are bounded (i.e., all eigenvalues of F„ are in the closed left- 
half plane); 

(2) The reference model (4) is stable (i.e., all eigenvalues of A,„ are in the 
open left-half plane); 

(3) <pjj is bounded ( i.e., all eigenvalues of F are in the closed left-half plane 

and any eigenvalues on the imaginary axis are simple); 

(4) (A, B, C) is Almost Strict Positive Real (ASPR), i.e., 

Tp(j') = C(.vl - A^j" B is strictly positive real. 
Then e* and AG are bounded and e* >0 and 

^y = yp -ym = Ce, ,^^ ) . 

See the Appendix in Part II for a proof of Theorem 2. 

This stability analysis shows that asymptotic tracking occurs and the adaptive 
gains remain bounded. It does not prove that AG ^0 . In fact, the gain ad- 
aptation laws (22) may not converge to the starred gains in (8); however, this is 
not required for the adaptive controller to achieve its goals. 

3 Conclusions for Part I 

We have reviewed our adaptive control theory here. This theory accounts for 
adaptive model tracking and for leakage of the disturbance term into the Q modes. 
However, the results require that the error system be minimum phase. In Part II, 
we will show how to modify the adaptive control with residual mode filters to deal 
with non-minimum phase systems. 
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Adaptive Control of Non-minimum Phase 
Modal Systems Using Residual Mode Filters: 
Part II 



Mark J. Balas and Susan A. Frost 



Abstract. In Part II, we extend our adaptive control theory to accommodate prob- 
lematic modal subsystems of a plant that inhibit the adaptive controller by causing 
the open-loop plant to be non-minimum phase. We will modify the adaptive con- 
troller with a Residual Mode Filter (RMF) to compensate for problematic modal 
subsystems, thereby allowing the system to satisfy the requirements for the adap- 
tive controller to have guaranteed convergence and bounded gains. Also, we will 
apply the above theoretical results to a simple flexible structure example to illus- 
trate the behavior with and without the residual mode filter. 



1 Introduction 

In Part II, we continue the development of the adaptive control approach. We will 
keep the consecutive equation numbering from Part I as well as the same reference 
list. We modify the adaptive control using the idea of Residual Mode Filters 
(RMF) introduced for fixed gain controllers in [6]. In this paper the RMF will be 
used to eliminate the effect of modes that prevent the almost strict positive real- 
ness (ASPR) of the overall system by being non-minimum phase. We have main- 
tained the same reference list here as in the previous paper to make both papers 
more readable. 



2 Residual Mode Filter Augmentation of Adaptive Controller 

In some cases the plant in (1) does not satisfy the requirements of ASPR. Instead, 
there maybe be a modal subsystem that inhibits this property. This section will 
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present new results for our adaptive control theory. We will modify the adaptive 
controller with a Residual Mode Filter (RMF) to compensate for the troublesome 
modal subsystem, or the Q modes, as was done in [6] for fixed gain non-adaptive 
controllers. Here we present the theory for adaptive controllers modified by 
RMFs. In a previous paper, we examined the RMF with adaptive control, but as- 
sumed that there was no leakage of the disturbance into the Q modes [7]. Here we 
will deal with the issue of disturbances propagating through these modes. 
Let us assume that (1) can be partitioned into the following modal form: 



A 
An 



\C Cr 



Q 

■,e>0 



B 

Bn 



Up + 



(1) 



Define X^ = 



Q. 



\ = 



Disturbance Generator 









B 




' r ' 




~ c 


, B = 




, r = 




> cl = 




' p 


A. 


' /' 


.^Q. 


' ^p 


Ca. 



, and 



or Zo = L^o as before in (2)-(3). 



|Zd = 

The Output Tracking Error and control objective remain as in (4)-(5), i.e. 

V y p yn 



-^0. 



However, now we will only assume that the subsystem \A,B,Cj is Almost 
Strictly Positive Real, rather than the full un-partitioned plant(A , B , C„j, and 
the modal subsystem {Aq,Bq,Cq) W\\\h& known and open-loop stable, i.e., Aq 
is stable. Also note that this subsystem is directly affected by the disturbance in- 
put. Recall that ASPR means CB > and P{s) = C{sl - A)~^ B is minimum 
phase. So, in summary, the actual plant has an ASPR subsystem and a known mo- 
dal subsystem that is stable but inhibits the property of ASPR for the full plant. 
Hence, this modal subsystem must be compensated or filtered away. 

We define the Residual Mode Filter (RMF): 



I ^Q — ^Q^Q + "Q^p 



yyQ ~^Q^Q 

And the compensated tracking error: 



ey^e^-jQ 



(2) 



(3) 



Now we let e^ = Xg — Xq and obtain: 



^Q ~ Q^Q ^Q^D 



(4) 
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^y = fiy - yg = CAX + CqXq - [CqXq + CqBq ] 
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(5) 



As in [l]-[2], we define the Ideal Trajectories, but only for the ASPR Subsystem: 

j i* = Ax* + But + Tuq 

[y.,=Cx.,=Q 

, This is equivalent to the Matching Conditions: 



(6) 



[C5* = 



(7) 



which are known to be uniquely solvable when CB is nonsingular. However, we 
do not need to know the actual solutions for this adaptive control approach. 

Ax = X — a:* 



Let < 



Am ^u -Ut 

Ay = e^, = CAx - CqBq 



Then we have 



Ax = AAx + BAu 
Ay =CAx-Cgeg 

because, from (33), j, =0. This system can be rewritten: 



Ax 



A 

An 



Ax 



Ay = [C -Cr 



Q 

Ax 

en 



Am = A 

Ax 
en 



Ax 
eg 



+ BAu + eTqUj) 



Now we have the following: 



Lemma. 



/ 


"A 


" 




B 


A = 






,B = 




V 





Aq_ 








c = [c -Cq] 



(8) 



(9) 



is ASPR if and only if 



{A,B,C) is ASPR. 
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Proof: CB=[c - Cr 



P{s) = C{sI-A)-'B 

(sI-AY 




[c 



c. 



CB>0 and 





(sI-AoY 



= C(sI-A)-'B = Pis) 
is minimum phase. End of proof. 

So there exists G* such that (A^ =A + BG*C,B,C) is Strictly Positive Real 
(SPR) when (A,B,C) is ASPR. Consequently, as is well known from the Kal- 
man-Yacubovic Theorem, there exists P,Q >0 such that 



Ac P + PA. 



c 



-Q 



[PB=C^ 
We now write the modified adaptive control law with RMF: 
Up = G^u^ + G„x„ + G^e^ + GJ^ 

ey = yp-yQ 

^ /~^ ^ 

with modified adaptive gains given by 

G, 



(10) 



(11) 



'eyU^K'X >0 



-eyX,„h,„;h„ >0 

-SyCy h/\ >0 



(12) 



GD=-^y^I>ho;hD>0 
Finally, we have the following stability result: 

Theorem 3. In (9), let iA,B,C) ASPR, Aq stable, (^^ bounded. Then the 
Modified Adaptive Controller with RMF in (19)-(20) produces e = y and e^ 

(l + V/'max ) 

ultimately bounded into a ball of radius R* = e- . My with exponential 



«V/ 



rate and bounded adaptive gains (G^ , G^, ) . 
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Proof: From (19), we have u = G^e + Gjy^j^ , so we can write 
Am = m — m« 

= [G,e,, + Go^B ] - [SIl]^o 

\ag,^g,-g: 

AGo^Go-(SIl) 
where <! AG = G - G* = [AG, AG^, ] ■ Then 



,w = AGj],Ac =A + BGX . 



(13) 



with ^ = 



Ajc 



From (20), we can see that 



G = AG = -eWh\h-. 







h 



D_ 



>0 



(14) 



Since (A,B,C) is ASPR, and by the lemma, so is (A,Z?,C) , we can we can use 
the following result from [8] where Vs FqM^ is bounded because the disturbance 
M^ =L(f)jj is bounded. 

Result. Consider the nonlinear, coupled system of dijferential equations, 

g = A^g + B[G{t)-G*)ri+ev 



ey = Cg 

G(t) = -e^rp^h - aG(t) 



(15) 



where G is any constant matrix and h is any positive definite constant matrix, 
each of appropriate dimension. Assume the following: 



i) the triple {A,B,C) is SPR, 
ii) there exists Mk > such that 



(o-Jo-^ 



< M^ , using the trace norm. 



Hi) there exists My> such that sup v(/) < M^ 
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^min 



iv) there exists a > such that a < — ^21E_ ^ am^ 



v) h satisfies \\h ' < 

•' II II2 



aM^j 



where p„i„, p^^x <^fs the minimum and maximum 



eigenvalues of P and q„i„ is the minimum eigenvalue of Q in the system 

iA^P + PAc=-Q 

\pB = C ^ 

Then the matrix G(t) is bounded and the state ^{t) exponentially approaches the 
ball of radius 

j?.^g '^^p!!i^ M, with e>Q. 

'^y Praia 

From this result, we have ^ is ultimately bounded into the ball of radius R., 
which leads to e^, =)'„=>'„—>'*= CA and Cq ultimately bounded as well. 
Therefore G = Gt+ AG is bounded, as desired. # 

(1 + JPj^Ti ] 

Consequently, the radius of the error ball /?« = e- , My is determined 

«V/'min 

by the size of e, which is related to the amount of disturbance leakage into the Q 
modes. It can be seen that, when there is no leakage of the disturbance into the Q 
modes (£"=0), the convergence is asymptotic to zero. 

Also, when T = B and Tq = Bq , it is possible to choose Si = and ^2 = -0 
in (34). Then, even if f = 1 , the tracking error will asymptotically go to zero. 



3 Simulation Results with RMF 

In this section we will apply the above theoretical results to a very simple flexible 
structure example to illustrate the behavior with and without the Residual Mode 
Filter. The structure has a rigid body mode and two flexible modes given by: 



P(S): 



1 + s 3 1 j^+.?'*+3s^+0s^+3s + l 



s^ s^+s + 1 s^+s + 2 s^ +2s^ +4s'^ +3s^ +2s^ 



This example can obviously be extended to have many more flexible modes. But we 
are only trying to illustrate the value of the RMF approach. More seriously complex 
flexible structures are being addressed but will have to await future papers. 

This plant has two non-minimum phase zeros at 0.422+0. 9543j and thus 
does not meet the ASPR condition. However, when the middle mode 

Pq(s) = —^: is removed, the plant becomes: 

5+5 + 1 
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l + s 1 s^ + 3s^ + 3s + 2 

— ^ + - 



P{s) = 

state space realization given by: 



which is minimum phase and has a 



[y,, = Cxp 



with A - 



10 

10 

1 

0-2-1 



, Y = B 



"o" 




2 





, c' = 


3 







3 


1 




1 



with CS = l,so CB is nonsingular. Therefore, {A,B,C) is ASPR. 
The reference model to be tracked is 



= (-1)x„+(1)m„ 



which is excited by steps generated by ii^ = (0)m„, . The matching conditions are 
known to be solvable, but their solution is not needed to apply the theory. 



The RMF generated by Pg (s) = — 

s +s + \ 



is represented by Aq 



1 
-1 -1 



^q-Bq 



C„=[-3 O] . And we see that C„B, 



Q"Q 



The adaptive controller given by (38) - (39) is implemented with /2u=10, h,^=\, 
/2e=10, /!d=100, and a=0. The disturbance is a nondimensional step of size 10. 
Setting £=1, we obtain figs. 1 and 2 from a MatLab/Simulink simulation. The 



Adaptive Output Tracking with RIVIF 




Fig. 1 Nondimensional output tracking response with adaptive controller augmented with 
RMF. 
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Adapti\e Gain Response 




10 12 
Time (sec) 



Fig. 2 Adaptive gains, Ge=error gain, Gd=disturbance gain. 

output tracking error is shown to converge to zero in fig. 1. The adaptive gains 
also converge in fig. 2. This illustrates the behavior of the adaptive controller plus 
the second order RMF. Without the RMF, the plant and adaptive controller are 
immediately unstable in closed-loop. 

4 Conclusions 

We have proposed a modified adaptive controller with a residual mode filter. The 
RMF is used to accommodate problematic modes in the system that inhibit the 
adaptive controller, in particular the ASPR condition. This new theory accounts 
for adaptive model tracking and for leakage of the disturbance term into the Q 
modes. A simple three mode example shows that the RMF can restore stability to 
an otherwise unstable adaptively controlled system. This is done without seriously 
modifying the adaptive controller design. 
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Appendix 

Proof of Lemma 1. The Linear Matching Conditions (8) can be rewritten: 

\as,+bs, = s,l^+h, 



where 5i = [^H S12 ^^j S2=[s2i S22 S^^l L„ = 

Hi=[o -ro] 



K 


B,n 


0" 





F,n 











F 



and 



H2Ac,„ 0] 



Suppose CB is nonsingular. Use the coordinate transformation W from Lemma 2 
in [1 1] to put {A, B, C) into normal form: 



y-- 

Z2 


= A]]}' + A]2Z2 +CBu 

= A2iy + A22Z2 




e.. 


there exists W = 


C 

wIpi 



such that WAW~' = A 



Ai A2 



WB = 



CB 





= B, and CW ' =[/„ 0] = C which impHes that 



SiL„ = WSiL,„ = WAW'WSi +WBS2 -WHi = AS^ + BS2 - H^ 
~CB~ 



AS^ + 







52 -//i 



and H2=CW~^WSi=CSi=[l 0]Si=S„ where Si =WSi 



From 



this we have that 



~H2 






'H2 




CB 




H~ 




L„ 


= A 




+ 




^2- 


a 


Jh_ 


m 




.^h_ 







L 


Ht_ 



which implies that 



\S2={CB)-'[H2L„+H^-(AiiH2+Ai2S,)] 

\ShL,n=A22Sh+(A2iH2-H,) 

Now, if (^22,^^) share no eigenvalues, it is well known [5] that we can solve the 



above for a unique 5^ and conversely, then S^ 



H, 



S2={CB)-\H2L^+H^-{Ai,H2+A,2S,)] and AS,-S,L^=H,. Since 
(A22,^;„) share no eigenvalues, this is the same as A22 sharing no eigenvalues 
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with A^, /^„ or F . But the eigenvalues of A22 from its normal form are known 
to be the transmission zeros of the open-loop system (A,B,C) ; see e.g. [13]. Thus, 
we have proved the result. End of Proof. 

Proof of Theorem 2 

It was already shown that e« and AG are bounded. To prove that e* ^0 , 

we must use the following version of Barbalat's lemma; see [19] pp. 210-211: 
Lemma: If f(t) is a real, differentiable function on (0,°°) with lim/(/) finite and 

— uniformly continuous, then lim — = . 

dt '^~ dt 

We have already seen that V{t)<Q; therefore V{t)-V{Q)= \ V{T)dT<Q or 

Jo 

0<y(r)<y(0) where 7(0) < 00. Hence \imV{t) is finite. Also, V{t) is 
bounded because 

y(f) = -(e/Ge*)<||e*||||e||||e*|| 

=lk*||||2lll|Are*+BAG;7|| 
II nil nil t- 'II 

<||e*||||2||(||Ac||||e*|| + ||fi||||AG||||;7||) 

and e* and AG are bounded by the previous argument via Lyapunov theory. Also 
7] is bounded since u^ is bounded, A^ is stable, e = Ce« is bounded, and ^^ is 

bounded. Thus V{t) = V(T)dT is uniformly continuous and Barbalat's Lemma 

Jo 

may be applied to yield: 

= lim V(t) = - lim e« Qe* . Since Q > , we have e* ^0 , as desired. End 

of Proof. 



Global Tracking Control Structures for 
Nonlinear Singularly Perturbed Aircraft 
Systems 



Anshu Siddarth and John Valasek 



Abstract. The problem of simultaneous tracking of both fast and slow states for a 
general class of nonlinear singularly perturbed systems is addressed. A motivating 
example is an aircraft tracking a prescribed fast moving target, while simultane- 
ously regulating speed and/or one or more kinematic angles. Previous results in the 
literature have focused on tracking outputs that are a function of the slow states 
alone. Moreover, global asymptotic tracking has been demonstrated only for a class 
of nonlinear systems that possess a unique real root for the fast states. For a more 
general class of nonlinear systems only local tracking results have been proven. 
In this paper, control laws that accomplish global tracking of both fast and slow 
states is developed using geometric singular perturbation methods. Global exponen- 
tial stability is proven using the composite Lyapunov function approach and an up- 
per bound necessary condition for the scalar perturbation parameter is derived. Con- 
troller performance is demonstrated through simulation of a combined longitudinal 
lateral/directional maneuver for a nonlinear, coupled, six degree-of-freedom model 
of the F/A-18A Hornet. Results presented in the paper show that the controller ac- 
complishes global asymptotic tracking even though the desired reference trajectory 
requires the aircraft to switch between linear and nonlinear regimes. Asymptotic 
tracking while keeping all the closed-loop signals bounded and well behaved is also 
demonstrated. Additionally the controller is independent of the scalar perturbation 
parameter nor does it require knowledge of it. 

1 Introduction 

This paper addresses systems that possess both slow and fast dynamics. This mul- 
tiple time-scale behaviour is either a system characteristic (for example, aircraft 
and flexible beam structures) or arises due to implementation of a fast controller 
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(for example, systems with fast actuators and/or fast sensors). The control objective 
is to develop a stable tracker for these two time-scale systems that would regulate 



both slow and fast states simultaneously. The singular perturbation approach lll3ll has 
been the foremost technique employed in the literature to examine the behaviour of 
these two time-scale systems. In this approach, the system dynamics are approxi- 
mated by two lower-order subsystems. The slow subsystem captures the dominant 
phenomena assuming that the fast variables evolve infinitely many times faster, and 
have settled down onto a manifold. The fast subsystem addresses the neglected phe- 
nomena, and assumes that the slow variables remain constant. It has been shown that 
the complete system behaviour can be approximated by the dynamics of the slow 
subsystem provided the fast subsystem is uniformly asymptotically stable about the 
manifold Jd llOll . These results of singular perturbation methods have made it the 



most favourable model-reduction technique in the control literature 1 4|] . 

The design of nonlinear tracking control laws for the slow variables using sin- 
gular perturbation methods has received a lot of attention in the past. The typical 
methodology is to design two separate controllers for each of the two subsystems, 
and then apply their composite or sum to the full-order system. A tracking control 
law is designed for the slow subsystem whereas a stabilizing controller is designed 
for the fast subsystem. This is done to restrict the fast variables onto a manifold. 
Global asymptotic tracking of the composite control structure is guaranteed only if 
the manifold is unique. This manifold is the set of fixed points of the fast dynamics 
expressed as a smooth function of the slow variables and the control inputs; hence 
it is not always unique. To enforce the uniqueness condition, previous studies in the 
literature have: 



1. Assumed that the system has a unique manifold! 

2. Considered nonlinear systems that have a unique manifold. This is satisfied by 
two time-scale systems that are nonlinear in the slow states and linear in the fast 



states m in 



For a general class of nonlinear systems such as aircraft, approximate approaches 
that guarantee local stability have been proposed. One approach is to consider the 



fast variables as control inputs for the slow subsystem. Reference II 1 211 used this 
approach to design nonlinear flight test trajectories for velocity, angle-of-attack, 
sideslip angle and altitude by using the fast angular rates as the control variables. 
This control was augmented with an outer-loop controller that determines the con- 
trol surface deflections needed to ensure that the angular rates track the output of 
the inner-loop. More recently the same concept has been employed for the control 
of generic reentry vehicles ITJ . Another approach proposed in Reference II 61 consid- 
ered the general class of nonlinear singularly perturbed systems and computed local 
approximations of the manifold that helped conclude local stability for the complete 
system. 

All of the approaches discussed above demonstrate slow state tracking either lo- 
cally or globally by restricting the fast states, and, they address the output tracking 
problem for two time-scale systems with fast actuators. But for systems whose dy- 
namics inherently possess different time-scales, both the slow and the fast states 
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constitute the output vector. For example, during air combat maneuvering an air- 
craft is typically required to track a fast moving target while regulating speed (slow 
variable) and/or one or more kinematic and aerodynamic angles. In this case the fast 
states cannot be restricted to simply stabilize onto a manifold. The reduced-order 
approach therefore appears to be inadequate for a general class of output track- 
ing problem. ReferenceyJ formulated optimal control laws to accomplish fast state 
tracking using invariant measures for systems with oscillatory fast dynamics. 

In this paper, state feedback control laws are developed for a general class of non- 
linear singularly perturbed systems to accomplish slow and fast state tracking simul- 
taneously. The paper makes two major contributions. First, the approach developed 
here employs the reduced-order technique without imposing any assumptions about 
the fast manifold. This is done by extending the previous work of the authors 1161] so 
as to not require computation of the manifold. Second, global exponential tracking 
is proved using the composite Lyapunov approach! H)]. From the stability analysis 
it is shown that this approach applies to all classes of singularly perturbed systems, 
with the global exponential stabilization results of a class of singularly perturbed 
systems being a special caseyfl. Additionally, the technique is independent of the 
scalar perturbation parameter and an upper bound on this parameter is derived as a 
necessary condition for stability results to hold. These results are demonstrated by 
simulation for a nonlinear, coupled, six degree-of- freedom model of the F/A- 1 8 A 
Hornet. 

The paper is organized as follows. Section[2]mathematically formulates the con- 
trol problem and briefly reviews the necessary concepts for model reduction from 
geometric singular perturbation theory. Control laws and the main results of the pa- 
per are detailed in Section[3] Section|4]presents simulation results, and conclusions 
are presented in Section|5] 



2 Problem Formulation and Model Reduction 

The following nonlinear singularly perturbed model represents the class of two time- 
scale dynamical systems addressed in this paper 

x = f(x,z)+g(x,z)u (1) 

ez = l(x,z)+k(x,z)u (2) 

""^ (3) 

where x S M™ is the vector of slow variables, z G M" is the vector of fast variables, 
u £ M.P is the input vector and y £ R'"+" is the output vector, e G R+ is the singu- 
lar perturbation parameter that satisfies < e << 1. The vector fields f(.),g(.),l(.) 
and k(.) are assumed to be sufficiently smooth and p> (m + n). The control objec- 
tive is to drive the output so as to track sufficiently smooth, bounded, time-varying 
trajectories, such that x(r) — > Xr{t) and z(?) -^ Zr{t) as f ^ oo. 
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2.1 Reduced-Order Modeling 

The singularly perturbed model considered in Eqs lll2l is expressed in the slow time 
scale t. In this time-scale the slow states evolve at an ordinary rate whereas the fast 
states move at a rate of O (i) . This system can be equivalently expressed in ihs.fast 
time-scale T such that the fast states evolve at an ordinary rate and the slow variables 
move slowly at a rate of (9(e) 

x' = e[f(x,z)+g(x,z)u] (4) 

z' =l(x,z)+k(x,z)u (5) 

where ' represents a derivative with respect to T = '-^ and fo is the initial time. Ge- 
ometric singular perturbation theory |6l] examines the behaviour of these singularly 
perturbed systems by studying the geometric constructs of reduced-order models 
obtained by substituting e = in Eqs lll2l and Eqs l4l5l This results in the Reduced 

Slow Subsystem 

X = f(x,z)+g(x,z)u (6) 

= l(x,z)+k(x,z)u (7) 

and the Reduced Fast Subsystem 

x' = (8) 

z' = l(x,z)+k(x,z)u (9) 

The reduced slow subsystem is a locally flattened space of the complete system 
fEqs lll2l) . Since the vector fields were assumed to be sufficiently smooth there ex- 
ists a smooth diffeomorphism that maps the complete system onto this locally flat- 
tened space. The set of points (x,z,u) G M'" x ffi." x K.'' is a smooth manifold .-#0 of 
dimension m + p that satisfies the algebraic Eq]?] 

^o:z = Zo(x,u) (10) 

This set of points is identically the fixed points of the reduced fast subsystem (Eq|9l). 
Thus the manifold ^q is invariant. The flow on this manifold is described by the 
differential equation 

x = f(x,Zo(x,u))+g(x,Zo(x,u))u (11) 

Fenichel||6|] proved that the dynamics of a singularly perturbed system of the form 
represented in Eqs lll2l is constrained within 0(e) of EqlTT]if the reduced fast sub- 
system is stable about ^q. If the dynamics of EqlTT]are locally asymptotically 
stable about the manifold, then it can be concluded that the complete system is 
also locally asymptotically stable. Global asymptotic stability conclusions about the 
complete system can only be made if the manifold is unique, which is a result from 
differential topology and center manifold theory ||6[]. 
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3 Control Formulation and Stability Analysis 

The central idea in the development is the following. If the manifold is unique and 
an asymptotically stable fixed point of the reduced fast subsystem, the complete 
system follows the dynamics of the reduced slow subsystem globally. Therefore, for 
a tracking problem addressed in this paper it is desired that this manifold lie exactly 
on the desired fast state reference for all time. This condition can be enforced if the 
nonlinear algebraic set of equations is augmented with a controller that enforces 
the reference to be the unique manifold. Additionally, this controller should also be 
capable of simultaneously driving the slow states to their specified reference. These 
ideas are mathematically formulated and analyzed in the following subsections. 

3.1 Control Law Development 

The objective is to augment the two time-scale system with controllers such that 
the system follows smooth, bounded, time-varying trajectories [xr(f),Zr(f)]^. The 
first step is to transform the problem into a non-autonomous stabilization control 
problem. Define the tracking error signals as 

e(0=x(f)-x,(r) (12) 

^(f)=z(0-z,-(0 (13) 

Substituting Eqs lll2l the tracking error dynamics are expressed as 

e = f(x,z)+g(x,z)u-x,. = F(e,^,x,.,z,-,Xr)+G(e,^,x,.,Zr)u (14) 

e| = l(x,z) + k(x,z)u-ez,- = L(e,i^,x,-,z,.,ez,-) + K(e,(^,x,-,z,-)u (15) 

The control law is formulated using the reduced-order models for the complete sta- 
bilization problem, which is obtained using the procedure developed in Section|2l 
Reduced Slow Subsystem 

e = F(e,^,x,-,Zr,x,-) + G(e,(^,Xr,z,.)uo (16) 

= L(e,^,Xr,z„0) + K(e,^,x„z,)uo (17) 

Reduced Fast Subsystem 

e' = (18) 

§' = L(e,(^,x,,z,.,z',)+K(e,<^,Xr,z,.)(uo + u/) (19) 

It is known that the fast tracking error E, will settle onto the manifold that is a func- 
tion of the error e and control input u, which may not necessarily be the origin. 
To steer both errors to the origin, the control input must be designed such that the 
origin becomes the unique manifold of the reduced slow system (EQs ll6ll7t . There- 
fore, the slow controller uq is designed to take the form 
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G(e,^,x„z,) 
K(e,^,x„z,) 



uo^ 



F(e,i^,x,.,z,.,x,-) 
L(e,<^,x,,z,-,0) 



AeC 



(20) 



where Ae and Af specify the desired closed- loop characteristics. With this choice of 
slow control, the reduced fast subsystem becomes 

e' = (21) 

§' = L(e,<?,x„z,-,z;)-L(e,§,x,-,z,-,0)+A^.<^+K(e,§,x„z,-)u/ (22) 

To stabilize the fast subsystem, the fast control Wf is designed as 



G(e,(^,x„Zr) 
K(e,(^,x„Zr) 



u/ 





L(e,(^,Xr,Zr,0)-L(e,<^,Xr,z,-,z;.) 



(23) 



Thus, the composite control u = uq + u/ satisfies 






■G(e,i^,Xr,z,) 
_K(e,|,Xr,z,) 


u = — 


■F(e,^,Xr,Zr,x,)' 
_L(e,^,Xr,Zr,z;.)_ 


+ 


' AeC' 


(24) 


assuming that the rank of 


G(.)- 
K(.). 


> {m + n). 





The complete closed-loop and reduced slow subsystem for this control law are 
given as 

(25) 
(26) 



and 



e 


== A(.e 


4 


-A^^ 


e = 


= AeG 


= 


-A^^. 



(27) 
(28) 



respectively. Observe that with the proposed control law the nonlinear algebraic set 
of equations (EgfTTIl have been transformed to a linear set of equations (Eq[28|. 
With the proper choice of At , it is guaranteed that i^ = is the unique manifold for 
both the complete and the reduced slow subsystems. Furthermore, this manifold is 
exponentially stable as can be deduced from the reduced fast subsystem 



e' = 



(29) 
(30) 



Remark 1. The control law proposed in Eq|24]is independent of the perturba- 
tion parameter e. Furthermore it is a function of zj. that implies that 
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the reference trajectory chosen for the fast states must be faster when 
compared to the reference of the slow states. Additionally, as for all 
singular perturbation techniques to work the closed-loop eigenvalues 
Ae and As must be chosen so as to maintain the time-scale separation. 

3.2 Stability Analysis 

Complete system stability is analyzed using the composite Lyapunov function 
approach ! idl . Suppose that there exist positive definite Lyapunov functions V (f, e) = 
e^e and W{t,^) = t,^t, for the reduced subsystems, with continuous first-order 
derivatives satisfying the following properties: 

1. y(f,0) = 0and7i||e||2<y(f,e) < 72||e||2 Vf G R+,e e R"',7i =^2 = 1, 

2. (Vey(r,e))^Aee < -aie^e, ai = 2|An,in(Ae)|, 

3. W(f,0) = and 73||§||2<W(?,§)<r4||§||2VfeK+,^eK«,73 = 74 = 1, 

4. {y^W{t,^)YA^^<-a2^^i, a2 = 2|An,in(A^)|. 

Next, consider the composite Lyapunov function v(f,e,<^) : R+ x M" x M" ^ R+ 
defined by the weighted sum of y(?,e) and W{t,t,) for the complete closed-loop 
system 

v(/,e,^) = (l-rf)y(/,e) + c/W(f,§), 0<^<1 (31) 

The derivative of v(/, e, i^ ) along the closed-loop trajectories Eqs l25l26l is given by 



V = (\-d){S/^vfe + d{y^wf^ 

V < -(1 -cf)aie'^e o-i^^ ^ 



V < 



(l-fif)ai 



faz 



(32) 
(33) 

(34) 
(35) 



Following the approach proposed in ReferenceP3ll, add and subtract 2av(?,e,^) to 
Eql35]to get 



V< 



{\-d)ai 



faz 



2a{\-d)V + 2adW-2av (36) 



where a > 0. Substitute in Eq[36|for the Lyapunov functions V (?, e) and W{t,t,) to 
get 



If e satisfies 



T r 



{\-d)ai-2a{l-d) 

'laz-2ad 



-2av 



(37) 
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e <e 



2a 



(38) 



and provided a\ > 2a, then from the definitions of a2, a, and d it can be concluded 
that the matrix in Eql37] is positive definite. Then the derivative of the Lyapunov 
function is lower-bounded by 



V < -2av 
Since the composite Lyapunov function lies within the following bounds 



or, 



711 



< v(r,e,§) <722 



where 7ii =min((l -d)'YudYi) and 752 = min ( ( 1 -d)'Y2,dY4) 
Lyapunov function can be expressed as 



V < -2a7ii 



(39) 



(40) 



(41) 



the derivative of the 



(42) 



From the definition of the constants 71 1, 722, and a, and invoking Lyapunov's Di- 
rect Methodic!], uniform exponential stability in the large 0/ (e = 0, (^ = 0) can be 
concluded. Furthermore, since the reference trajectory Xr{t) and z,.(/) is bounded, it 
is concluded that the states x(f ) — *■ x,.(r) and z(r) — *■ Zr(?) as / — > 00. Since the matrix 

Gf )1 

„;', is restricted to be full rank, examining the expression for u in Eq|24]it is 

concluded that u G iloo. 

Remark 2. Recall that for the special case of state regulation the system dynamics 
in Eqs ll4ll5] become autonomous. In such a case, the result of global 
exponential stability is obtained with less-restrictive conditions on the 
Lyapunov functions y(e), W((^), and consequently v(e,(^). Similar 
conclusions were made in Reference yfl for the stabilization problem 
of a special class of singularly perturbed systems where the control 
affects only the fast states. Note that for the special class of systems 
considered in ReferencelJ], the non-diagonal elements of the matrix 
in Eql37] are nonzero, and the bound on the parameter e is slightly 
different. 

Remark 3. From Eq[37l a conservative upper bound for a is a < ^, and conse- 
quently e* « ^. Therefore, qualitatively this upper bound is indirectly 
dependent upon the choice of the closed-loop eigenvalues. 
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4 Numerical Simulation 

The purpose of the example is to demonstrate the methodology and controller per- 
formance for an under-actuated, nonlinear, singularly perturbed system. The system 
studied is a nonlinear, coupled, six degree-of- freedom F/A-18A Hornet aircraftlSl]. 
The combined longitudinal-lateral/directional maneuver requires tracking of the fast 
variables, in this case body-axis pitch and roll rates, while maintaining zero sideslip 
angle. Closed-loop characteristics such as stability, accuracy, speed of response and 
robustness are qualitatively analyzed. The maneuver consists of an aggressive verti- 
cal climb with a pitch rate of 25 deg/sec, followed by a roll at a rate of 50 deg/sec, 
while maintaining zero sideslip angle. The Mach number and angle-of-attack are 
assumed to be input-to-state stable. The initial conditions are a Mach number of 
0.4 at 15,000 feet, an angle-of-attack of 10 deg, and elevon angle of —11.85 deg. 
All other states are zero. The F/A-18A Hornet model is expressed in stability axes. 
Since it is difficult to cast the nonlinear aircraft model into the singular perturbation 
form of EqlT]|2] , the perturbation parameter e is introduced in front of those state 
variables that have the fastest dynamics. This is done so that the results obtained for 
e = will closely approximate the complete system behaviour (with e = 1). This is 
called the forced perturbation technique, and is commonly used in the aircraft liter- 
ature y, JJil- Motivated by experience and previous results, the six slow states are 
Mach number M, angle-of-attack a, sideslip angle /3 and the three kinematic states: 
bank angle ^, pitch-attitude angle 6, and heading angle i//. The three body-axis an- 
gular rates {p, q, r) constitute the fast states. The control variables for this model are 
elevon 5e, aileron da, and rudder 5,- and are assumed to have sufficiently fast enough 
actuator dynamics. The convention used is that a positive deflection generates a neg- 
ative moment. The throttle T) is maintained constant at 80%, because slow engine 
dynamics require introduction of an additional time-scale in the analysis; this is a 
consideration which is beyond the scope of this paper. The aerodynamic stability 
and control derivatives are represented as nonlinear analytical functions of aerody- 
namic angles and control surface deflections. Quaternions are used to represent the 
kinematic relationships from which the Euler angles are extracted. The details of 
these relationships are discussed in Reference Iil5i1 . 
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Fig. 2 Control Surface Deflections 
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Fig. 6 Three-Dimensional Trajectory 



Results and Discussion 

Simulation results in Figures [T]|6] show that all controlled states closely track their 
references. At two seconds the aircraft is commanded to perform a vertical climb, 
and after eight seconds the pitch rate command changes direction and Mach num- 
ber drops. The lateral/directional states and controls are identically zero until the 
roll command is introduced at time equals 15 seconds. Observe that all of the states 
asymptotically track the reference. Figure [2] shows that the elevon deflection re- 
mains within specified limits 151] throughout the vertical climb, and the commanded 
roll produces a sideslip angle which is negated by application of the rudder. The 
aileron and the rudder deflections remain within bounds while the aircraft rolls and 
comes back to level flight. The maximum pitch-attitude angle is 8 1 deg, maximum 
bank angle is 81 deg (Figure Hjl, and the maximum sideslip error is ±4deg. The 
quaternions and the complete trajectory are shown in Figures |5] and |6] respectively. 
From Figure |6l note that after completing the combined climb and roll maneuver, 
the aircraft is commanded to remain at zero sideslip angle, roll rate, and pitch rate. 
It then enters a steady dive with all other aircraft states bounded. The controller re- 
sponse is judged to be essentially independent of the reference trajectory designed. 
The robustness properties of the controller are quantified by the upper bound e* . For 
this example, the design variables are d — 0.5, ai — 10, a — 2, and a2 = 15, so the 
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upper bound becomes e* =7.5. Therefore for all e < e* global asymptotic tracking 
is guaranteed and in this case e = 1 . 



5 Conclusions 

A control law for global asymptotic tracking of both the slow and the fast states for a 
general class of nonlinear singularly perturbed systems was developed. A composite 
control approach was adopted to satisfy two objectives. First, it enforces the speci- 
fied reference for the fast states to be 'the unique manifold' of the fast dynamics for 
all time. Second, it ensures that the slow states are tracked simultaneously as desired. 
Stability of the closed-loop signals was analyzed using the composite Lyapunov ap- 
proach, and controller performance was demonstrated through numerical simulation 
of a nonlinear, coupled, six degree-of- freedom model of an F/A-18A Hornet. The 
control laws were implemented without making any assumptions about the nonlin- 
earity of the six degree-of-freedom aircraft model. Based on the results presented 
in the paper, the following conclusions are drawn. First, both positive and nega- 
tive angular rate commands were seen to be perfectly tracked by the controller and 
consistent tracking was guaranteed independent of the desired reference trajectory. 
Second, throughout the maneuver the controller demonstrated global asymptotic 
tracking even though the desired reference trajectory requires the aircraft to switch 
between linear and nonlinear regimes. This robust performance of the controller was 
shown to hold for all e < £* = 7.5. Third, all closed-loop signals were bounded and 
the control surface deflections computed were smooth and within specified limits. 
Fourth, this technique does not require the knowledge of the perturbation parameter 
e. This is an important consideration for systems such as aircraft, where quantifying 
this parameter can be difficult. 
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Motion Planning for a Fixed-Wing MAV 
Incorporating Closed-Loop Dynamics Motion 
Primitives and Safety Maneuvers 

Michael Gros, Moritz Niendorf, Alfred Schottl, and Walter Fichter 



Abstract. In the following a new two-staged motion planning algorithm with air- 
craft safety guarantees for obstacle cluttered environments is presented. The first 
planning stage consists of a probabilistic roadmap global planner that implicitly 
accounts for kinematic constraints of the plant and generates waypoints. These are 
used as an orientation marker for the on-line sampling-based second planning 
stage incorporating motion primitives based on the closed-loop dynamics of a 
nonlinear 6 degrees of freedom model of a fixed- wing mini aerial vehicle (MAV). 
Limitations in turnaround time during on-line execution are accounted for by lim- 
iting the planning depth of the tree. Safe trajectory traversing is assured with the 
introduction of safety maneuvers in the horizontal and vertical plane that have to 
be feasible at every node. Results of simulation runs are presented for a scenario 
including a narrow passage and an unknown obstacle. 
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1 Introduction 

Motion planning for fixed-wing MAVs in obstacle cluttered environments is a 
challenging task, especially when aircraft safety has to be assured continuously 
and in case of a high density of obstacles and narrow passages. Moreover, this be- 
comes even more severe coping with unknown static and/or dynamic obstacles 
during runtime. The main purpose of this work is to combine the advantages of re- 
cently introduced planning algorithms to a realistic real-time planning framework. 
To this end, improvements are introduced in the fields of planning safety due to 
safety maneuvers and the generation of motion primitives (MPs) with a closed- 
loop model. Further, a tree-based local planner that combines advantages of depth- 
first and breadth-first search techniques as well as the incorporation of local sensor 
information for obstacle avoidance is presented. 

A popular strategy to solve the planning task in obstacle cluttered environments 
is to decompose the motion planning problem into a coarse, discretized global 
planner, and a finer local planner that takes into account dynamic constraints [1]. 
Global planners employ roadmap-based representations whose query phase uses 
graph search algorithms, e.g. the A* algorithm [1], to produce a waypoint path 
connected with straight line segments. As representation of the dynamics of vehi- 
cles planners often sample motion primitives in order to reduce computational ef- 
fort [1,2,3]. Frazzoli [2] introduced the so called maneuver automaton, a finite 
state machine that interconnects two classes of motion primitives, namely trim tra- 
jectories and maneuver trajectories. Hwangbo et al. [1] combined a grid-based 
global planner with a simple tree-based local planning framework and generated 
motion primitives for a fixed-wing UAV in 3D-slalom scenarios. 

Obstacle avoidance during runtime is crucial when unmanned aerial vehicles 
(UAVs) have to fly through dynamic environments. Thus it makes sense to di- 
rectly embed range information such as from a laser range finder (LRF) or a radar 
distance sensor into the motion planner. Such an approach is reported in [4] and 
[5] in the framework of rapidly exploring random trees (RRTs) and potential func- 
tions, respectively. Most safety measures considered in planning algorithms are 
applied to rotary wing MAVs [2,6]. The problem is more challenging in case of 
the nonholonomic constraints of a fixed-wing MAV. An incorporation of safety 
maneuvers for fixed-wing aircraft into the planning process cannot be found in 
current planning algorithms. 

In order to design an algorithm that is tailored to real-time application, the fol- 
lowing measures were carried out: A modification of the global planner so that it 
can cope with restrictions on the flight path angle and heading difference angle; 
the capability of avoiding unknown static and dynamic obstacles is enhanced by 
supporting the local planner with available range sensor information. To provide a 
fixed turnaround time during a planning step of the local planner, the planning ho- 
rizon and the number of nodes stored to be considered for the next planning step, 
are limited; safety maneuvers at any node of the planning tree must be considered. 
These points are functionalities that are added with the respect to the original work 
of Hwangbo et al. [1]. 
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The paper is outlined as follows. Section 2 gives a detailed description of the 
nonlinear closed-loop dynamics of the fixed-wing aircraft and the generation of 
motion primitives. The probabilistic roadmap based global planner is presented in 
section 3, the tree based local planner, including safety maneuvers and the simu- 
lated LRF, is introduced in section 4. A simulation scenario with an unknown ob- 
stacle and a narrow passage is discussed in section 5. Summary and outlook are 
given in section 6. 

2 Simulation Model and Motion Primitives Generation 

2.1 Problem Formulation 

For a nonlinear system, x = /(x(f),u(0), x(0) ^Xq, where x g /?" represents a 

state in the state space X, Xq being the initial state at time f=0 and ue /?'" being 
the input to the system, motion planning can be stated as path planning in a state 
space with first-order differential constraints as opposed to the classical path plan- 
ning problem that is formulated in the configuration space; a big advantage is the 
incorporation of kinematic and dynamic constraints of a system, i.e. the incorpora- 
tion of finite accelerations in the simulation of a system in an obstacle cluttered 
environment can be crucial for collision avoidance due to the system's inertia. 

The dimension of the configuration space C is the number of the degrees of 
freedom of a body, thus a configuration qe C determines a rigid body's position 

and attitude; the configuration q and its first order derivative are included in the 
state X. Constraints imposed on x by kinematic and dynamic bounds on the system 
and by static and moving obstacles can be expressed by defining a subspace 

Xf^gg c R" that contains all feasible states; moving obstacles are represented as 
static obstacles at time f, it is assumed that their trajectory is fully known. Con- 
straints on the input u are covered by the subset U c 7?™ , leading 
to x(f ) G XjYgg (0; u(f) G U are selected by a local planner. The system is run from 
the initial state Xo until the state reaches a terminal set Xg. Associated to Xg is the 
stopping time tg:=inf(t: Xg g Xg). The purpose of this framework is to plan a trajec- 
tory across an obstacle cluttered environment, thus for Xg only position informa- 
tion is specified. 

2.2 Simulation Model with Motion Primitives 

A nonlinear 6 degrees of freedom (6-DoF) model of a fixed-wing MAV is used for 
this work. Aerodynamic data for the model was generated using Digital 
DATCOM [7] and validated with flight data. The state vector x consists of the po- 
sition in an Earth-fixed reference frame (x.y.z), Euler angles (0,6,y/), body-fixed 
velocities {u,v,w) and the aircraft angular velocity vector (p,q,r). The open-loop 
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control input u to the system is represented by u= {da,Si,,5r,5,), the subscripts indi- 
cate aileron, elevator and rudder as well as throttle. Instead of the application of 
the open-loop input u over a specified time interval during the generation of Mo- 
tion Primitives (MPs), a simulation with a closed-loop system is preferred due to 
its well-known stability advantages compared to an open-loop representation. As 
can be depicted from figure 1, flight path variables are chosen as reference signals 

r(/) G U ,. U ,. c 7?™' ; closing the loop, the dimension of the input vector can be 

reduced by one to m,=3, assuming that zero body-fixed lateral acceleration a,, is 
desired. This is achieved via feedback of Uy to the rudder input J,. . The closed- 
loop system is now defined as 



x = /(x(0,r(0), x(0) = Xo, r = (0,„„„/,„„„y,„„,), 



(1) 



where (^m,,, is the commanded roll angle. For a small angle of attack a and sideslip 
angle P, (p matches the bank angle jx. Thus, assuming 0~ /u, serves as the refer- 
ence signal of the lateral motion. The commanded flight path angle Yen, and the 
commanded absolute speed Vcom serve as reference signals of the longitudinal mo- 
tion. The full feedback policy, including turn coordination and turn compensation, 
can be deducted from figure 1. For motion primitive generation it is defined that 
the MAV should be capable to execute straight flights as well as turns at different 
turn rates, both with different flight path angles. Thus, the elements of r are 
limited to a finite number of values. The commanded absolute speed Vcom is held 
constant while providing n^ different (Z>„j,„ and Mj, different }fo,„, resulting in ni-= n^- 
n^ different sets of r, namely r„ with ;=l...nr- 
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Fig. 1 Structure of the nonlinear closed-loop dynamics of our fixed-wing MAV. 



2.3 Motion Primitives Generation 



The sampling of motion primitives (MPs) decomposes the input of a given dy- 
namic system into a finite number of control actions. The consecutive execution of 
MPs is determined by an optimization process in the local planner. Several ways 
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to generate motion primitives exist including the recording of open-loop actions 
by a pilot during a maneuver or the application of an optimal control policy as de- 
fined in [2] . 

This work presents a straight forward way of MP sampling from the closed- 
loop system (1) in the interval t g (/„fy] with the initial state x(f5)=Xs represented 
by rs=ri(f5). In contrast to the work of Frazzoli [2], this work introduces two dif- 
ferent classes of MPs, namely steady-state MPs and non steady-state MPs which 
apply for small changes in the heading angle X- Steady-state MPs begin and end in 
steady states, whereas in non steady-state MPs at least one boundary state is not a 
steady-state'. For steady-state MPs a constant reference signal r(/)= r[=const. is 
applied during the interval t g (f,,//]. The maximum of the settling time f,,., of the 
underlying controllers for ^^j^ assures that the commanded reference almost has 
been reached, thus leading to 

lim (^,„„ - ^) < e^ , lim ( Ycom -7)<ey, t,„ = max(f ,^ , f ), (2) 

where e defines the maximum allowed controller error. As a consequence, every 
motion primitive ends in a quasi steady state x^ I x^ = , excluding position; un- 
steady transitions between two consecutive MPs are prevented. The final state Xf 
can be associated with vector Tf =ri(//) and for the initial state of a MP, Xs is asso- 
ciated with rs=ri(/j). With a limitation time of the upper boundary t„ax and lower 
boundary /„,„ to generate motion primitives of roughly the same length, tf is con- 
strained to f„^. > fy> /^,„. 

With the steady-state MP class small heading changes cannot be realized; to 
enable those maneuvers in the horizontal plane (j'5=Jf=0°), non steady-state MPs 
are introduced. Figure 2 depicts non-steady state MP generation. From an initial 
roll angle of ^s{ts)=0°, a reference roll angle ^sub{t])i^ 0° is commanded in the in- 
terval ts<t]< t„gjg, followed by the application of ^(/2)=0° with t„„^g<t2< tset{^=0°). 
The members of ^^uh are chosen from ^c-om in such a way that only reference signals 
that command a small (Z* are taken in order to produce a small A;^. A number of n^ub 
steady-state MPs with (Z>5((s)=0° and ^fitf)=^sui, are used for the generation of non- 
steady-state MPs. Those MPs are then divided into n„„je different nodes at time in- 
tervals of t„„ag=tset{0suhVnnode leading to n„„ji,-l non steady-state intermediate nodes 
per MP. Subsequently n„ode-l simulations are conducted with ^comiti) =0sub to each 
intermediate node, which are placed in intervals of t„ode x j, j=i---n„ode-^; from 
each intermediate node, the horizontal roll-angle reference signal ^com{t2)=0° is 
applied until eq. (2) is satisfied. 



MP naming was slightly changed compared to [2] due to differences in the classification. 
The set of steady-state MPs contains both trim and maneuver MPs, whereas non steady- 
state MPs represent pieces for maneuvers to refine the maneuver library. MPs based on 
constant, respectively piecewise constant controller references were used rather than op- 
timal control laws as in [2] . 
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Fig. 2 Non steady-state motion primitive generation for the MAV with n__ 
non-steady-state MPs result from 1 steady-state MP. 



=5. n 



Each node to node connection in this class is considered as a single non steady- 
state MP in the tree based planner, resulting in riunsteady =2' Mnod? -1 MPs with a non 
steady initial and/or final state for each member of 0suh- Assuming that for the ini- 
tial states Xs and end states Xf of steady-state MPs any combination of reference 
vectors r, with (=l...nrCan be applied, the total number n,„,ai of steady-state and 



non-steady state MPs to generate is defined by n,g,ai=nr + n^ub 'i^i 



unsteady' 



1). 



3 Probabilistic Roadmap Global Planner 

A common strategy to solve the motion planning problem in obstacle cluttered en- 
vironments is the decomposition into a coarsely discretized global and a finer dis- 
cretized local planning problem. The geometric representation of obstacles in both 
the global and the local planner is provided by axis aligned bounding boxes in the 
Earth-fixed reference frame, straight line segments in the global planner are sur- 
rounded by oriented bounding boxes (OBB). In this setup collision detection can 
be implemented efficiently using the method provided in Ref. [8]. 



Algorithm 1. PRM Roadmap Construction (nNodes) 



1 : nodes <— sample nNodes random configurations 

2: for all nodes 

3: find ^_f!efl!ce.st nearest neighbors 

4: if collision check and y< %^^^^ then roadmap • 



- edge; end; 



A probabilistic roadmap (PRM) algorithm is used as global planning method to 
produce waypoints that are connected with straight segments for the local planner 
a priori. Reference [9] delivers an introduction to the basic PRM algorithm. As we 
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see the PRM algorithm can be extended to geometric constraints for nodes and 
edges to implicitly satisfy the kinematic constraints of the aircraft. 

The PRM algorithm is divided into a construction and a query phase. As can be 
seen from algorithm 1 , roadmap construction begins with the sampling of nNodes 
different random configurations, in our case only the position {x,y,z) of a node 
with Euclidean distance as metric is considered, since the system representation 
can be seen as a mass point representation for that purpose. For every node a near- 
est neighbor search is conducted. The node structure is realized by a kd-tree repre- 
sentation [10]. Several constraints have to be satisfied during the construction 
phase before an edge connection between two nodes is possible: 

• Edges included in the surrounding OBB are checked for collision to provide a 
collision free volume for the execution of the local planner. 

• A constrained flight path angle y„ax of the MAV requires a constraint on the 
slope of the edge namely y< y„ax- 

Algorithm 2. PRM Roadmap Query 

1 : PRM Roadmap Construction (qo,q,) 

2: shortest path(q„,qj <— modified A* between q„,q,witli Dubins metric 

Algorithm 2 depicts the necessary steps in the query phase. Initial configuration 
qo= {xo,yo,Zo,Zo,7o,M' i-S- position, heading, flight path angle, and the goal con- 
figuration qg= (Xg,yg,Zg,Zg.7g,^g)7 with a given position and arbitrary Zg. Yg have to 
be connected to their k nearest neighbors in the roadmap; the roll angle (j) is left 
arbitrary in the entire global planning process. As a second step a modified A* 
graph search algorithm is applied to find the shortest path from qo to qg in the 
roadmap. The basic A* algorithm [11] is an informed search algorithm that is 
proven to be complete. The modification made for this algorithm further adds 
flight path constraints limiting 

• the maximum allowed heading difference AXmax due to a limited turning radius 
and 

• the minimum length of a straight line segment L„,„ by approximating it as the 
length of an arc piece by L™„ = 2-AXmax ' Rmin ' Csafety, where Csafety is a safety fac- 
tor and Rynin is the turning radius determined by the motion primitive with the 
smallest turning radius. 

Only if these constraints are satisfied the specific node is included into the A* 
graph search and the heuristic in the form of a modified Dubins heuristic, as will 
be described below, is evaluated. 

For a vehicle with minimum turning radius in the plane it can be proven that 
the shortest cormection of a start and a goal configuration is always a part of a set 
with 6 path types, with mostly 3 segments consisting of arcs and straight seg- 
ments. Those paths are referred to as Dubins paths. Here an analytical solution for 
Dubins paths from an initial configuration with position and heading to a goal po- 
sition with the final heading left arbitrary is used [12]. An extension to the Dubins 



254 M. Gros et al. 

heuristic for an approximation of tlie trace in a 3 -dimensional workspace is pre- 
sented in Ref. [1] and is used in this work in a modified form, as can be seen in al- 
gorithm 3. Additionally, after the Dubins heuristic value for the horizontal plane 
Lh,Dubms is obtained, a check of the flight path angle constraint y^ax is conducted in 
step 3 of the algorithm. As long as this constraint is not satisfied, full turns with 
the minimum turning radius /?„,„ of the MAV are added to the Dubins heuristic 
value. As modification to the algorithm of Ref. [1], more accurate trace lengths for 
a fixed-wing aircraft are obtained by adding the height difference Az=Zg-Zi to the 
Dubins heuristic for the vertical plane LyOubms in step 4. 

Algorithm 3. 3D Dubins Metric (qi,q,) 

1: Li^[^i^„ <— Compute Dubins Heuristic in Horizontal Plane (qi,q2) 

2: h < ^,Oifhm, 

3: while Az/h > sm(y^^^J do h->—h + 2nx R^^_^; end; 



It is possible that the modified Dubins heuristic overestimates the distance with 
the consequence that the A* triangle equation does not hold; thus an optimal solu- 
tion is not guaranteed. However, in interaction with the probabilistic roadmap al- 
gorithm the modified Dubins heuristic seems to produce a better path with respect 
to the kinematic constraints of a fixed- wing aircraft than the admissible Euclidean 
distance heuristic; a detailed study evaluating this issue is still due. 

Once A* returns a shortest path according to the 3D Dubins heuristic as a se- 
quence of nodes from qo to qg, an additional post processing step is executed that 
attempts to connect non-neighbored nodes directly to each other. This automati- 
cally leads to an optimization of the path length, i.e. a shortened path, if a connec- 
tion is collision free and feasible with respect to the above mentioned constraints. 

4 Tree-Based Local Planner with Motion Primitives 

This new local planner builds a tree from an initial state Xo to a goal region around 
the goal state Xg, with only position specified a priori, by the interconnection of 
MPs. PRM waypoints serve as intermediate goal regions which are represented by 
a sphere with a certain radius around Xg. Due to the discrete nature of MPs it is un- 
likely that a goal state is reached exactly; however, there is a trade-off. A too nar- 
row goal region decreases the chance that a waypoint is hit; a too wide goal region 
decreases the guidance "property" of waypoints for the local planner in an obsta- 
cle cluttered environment. A goal region of the size less than the minimal turning 
radius has been found acceptable through simulation of the model considered here. 
The planner performs an informed search based on a partially greedy cost func- 
tional J as opposed to Ref. [1] where a planner based on a greedy search is used. It 
is desired to obtain depth-first behavior in the free space that results in fast search- 
space exploration and a breadth-first exploration when close to obstacles or in the 
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proximity of a goal region to minimize the probability to end up in a local mini- 
mum. The structure of the local planner is given by algorithm 4. 

From chapter 3 it is clear that every node in the local planner can be sufficiently 
represented in the configuration space with q={x,y,z,Z,%^), even though the sys- 
tem of eq. (1) is defined in the state-space. For a MP that starts at q^, ends at Qf 
and an intermediate goal node qi^, from the PRM path, the cost functional J is de- 
fined as 

•/(qf) = /(qs'qf)+g(qf'qint)+Mqf,qobs)' (3) 

where /(qs,qf) is the trace length of the MP and g(qf,qint) is the greedy cost-to-go 
with the 3D Dubins heuristic between qf and qi,,,. The term /!(qf,qobs) represents the 
value of a potential function in the line of sight of the MAV between the position 
coordinates of qj and an obstacle position qobs- In the potential function 

Mqf ' qobs ) = -^LSF • max ^«^ /I /(I qf -q^bs 1-1)) , (4) 

qobs is determined by the simulation with a laser range finder (LRF) similar to Ref. 
[4], equipped with a beam of limited range cIlrf, variable pitch angle Ad and 
azimuth angle A iff. The LRF simulation produces a grid of size j at each end con- 
figuration qt of a MP with distance information to the next obstacles that intersect 
with the beam due to its pivoting; for simplicity reasons an ideal sensor was as- 
sumed without false hits or latency. The potential function always accounts for the 
worst case, thus the obstacle position qobs with smallest distance to qt was chosen 
from the grid. 

In the tree building process, every MP is seen as a new branch of the tree origi- 
nating from a node and will be incorporated into the planning process only if one 
of four subsequent safety maneuvers of the MAV in the horizontal or vertical 
plane is possible and if no collision occurs. Safety maneuvers were not simulated 
explicitly but represented by OBB placeholders and collision checked for every 
MP. In total there are two safety maneuvers for each the horizontal respectively 
the vertical plane. It is assumed that a safety maneuver is conducted at the edge of 
the flight envelope with a minimum turning radius that is by far lower than the 
minimum radius of steady-state MPs. 

Similar to the A* algorithm this planner stores a list of nodes, named open_list, 
sorted in the order of their cost. Conversely to A*, this list is finite to limit the 
computational effort at each cycle. Only nodes that are members of the open_list 
are candidates for a further expansion of the tree. The node with the lowest cost is 
labeled current_node and thus is added to the tree. In the next step, the cost- 
functional of the nodes at the end of the branches of the current _node is evaluated. 
Subsequently, all collision-free nodes are added to the open_list which is then 
sorted again (line 9-10). If the size of open_list is bigger than allowed, nodes with 
the worst cost functional are removed. Therefore the size of the open_list is al- 
ways a trade-off between computational effort and a convergence to the optimal 
solution since it is proven that the basic A* algorithm is complete [10]. 
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current_node <— open_list(l) <— node^^^ <— initial node at state x„; age = 0; 
while not in goal area around x^; 
if in local goal area around q.^, 

qj^j, local goal <— next waypoint from PRM; 
open_list: for all nodes; calculate cost / end; sort; limit size; 
current_node <— open_list(l): 
else 

age '^age +1; 

current_node: for all branches; calculate cost /; collision/safety maneuver check; end; 
10: open_list: save all feasible nodes of branches (from line 9); sort; limit size; 

1 1 : current_node <— open_list( 1 ) ; 

12: check if local or global goal area is reached; 

13: if age-age,,^ > planning _depth 

14: paf/j<— recursively find path from current_node to current MAV configuration node^^y, 

15: node^^y <— move MAV to the next steady-state node; 

16: if node^^y = current_node then execute safety _maneuver; end; 

17: open_list: delete branches whose origin nodes possess timestamp < age; 

18: «^e,,w^ age; 

19: end 



A limitation of the computation time is achieved by a limitation of the number 
of newly added motion primitives in each planning step. This is realized by the 
variable planning _depth. After reaching the plannmg_depth, the MAV trajectory 
is recalculated recursively in the tree from the node with the best value, i.e. cur- 
rent_node, to the actual MAV node nodeMAv and the MAV is propagated to the 
next steady-state node (line 15). In case of a planning failure (line 16) a safety 
maneuver is executed. Parts of the tree that cannot be reached anymore by the 
MAV are pruned away. 

The use of a modified Dubins heuristic for the greedy part of the cost functional 
gives a more accurate estimation of the vehicle trajectory compared to the Euclid- 
ean distance. This maximizes the influence of distance deviation and minimizes 
the deviation in vehicle orientation when farther away from the goal region to 
achieve a depth-first exploration in the free space; if the tree advances to a goal 
region in a narrow passage, the greedy cost-functional value of branches that pos- 
sess a small deviation in orientation to the goal region will be lower and the lim- 
ited length of the open_list causes a widening of the tree at the same hierarchy 
level which resembles breadth first behavior. The LRF provides local obstacle in- 
formation that can be used in the repulsive potential to push the vehicle trajectory 
towards the free space Xfree when in the proximity of obstacles; MP that point to 
close obstacles are penalized heavily by the potential function and vice versa. 



Motion Planning for a Fixed- Wing MAV 



257 



5 Simulation Test Runs 

For the motion primitive generation of the MAV a set of Mr= 35 different reference 
vectors r were chosen as shown in table 1 . To enable the aircraft to execute small 
heading changes in the range of 1°< Ax< 15°, the number of intermediate nodes 
for non steady-state MPs was chosen to n„oje=5, resulting in nunsteady='^ non steady- 
state MPs per member of ^,„i=[-20°,-10M0°,20°] with M.™t=4 members. 

This led to a total number of m,om;=1257 applicable MPs; 170 MPs are excluded 
due to a violation of the time constraint tmax, leading to 1087 MPs that have to be 
administered and applied by the tree-based local planner. 

Symmetry properties of the aircraft dynamics in the x-z plane could be used to 
reduce the MP set; a saving in cost functional evaluations due to the x-z plane 
symmetry could only be achieved for symmetric flight initial conditions, 
i.e. (lj{ts)=Q°, and only for the greedy part of 7. Due to the minimal achievable sav- 
ings the MP set is not using symmetry properties. 

Constraints to the global planner were chosen to J^oi— 10°,zl;i'maj:=60°, 
/?„,„=38m. LRF parameters were set to JMf=80m, A0 = [-20°, 20°], A y^ 
[-45°,45°]. 

Table 1 Reference Signal Discretization 
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Fig. 3 The PRM solution of an exemplary scenario with a narrow passage in the order of 
/?„,„. Due to the narrow passage, PRM parameters had to be chosen to nNodes=10000, 
nNeighbour=500 to provide a dense sampling of the map. 
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(a) Local planner: time = 3 x planning_depth 
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Fig. 4 Every figure shows the whole tree including any former members of open_list. Free 
space is traversed fast by the planner. The tree grows in breadth in front of the narrow pas- 
sage, as shown in figures (a)-(b). Firstly, in (a) the best node lays outside of the narrow pas- 
sage, but in (b) a path inside the narrow passage is explored that has the best value for the 
cost functional J. Afterwards it is switched to the next waypoint. Figure (c) depicts the fin- 
ished motion plan from Xq to Xj with planning_depth= 10, max(open_list)=20. A solution of 
the local planner in the presence of an unknown obstacle (red) is shown in figure (d). The 
path generated by the PRM planner is the same as in figure 3. 



A simulation in an urban environment with focus on narrow passages and un- 
known obstacles was chosen to demonstrate the capabilities of the introduced 
planning framework. 

Figure 3 shows the PRM solution and the smoothed path in an environment 
containing a narrow passage between buildings with a passageway width less than 



Motion Planning for a Fixed- Wing MAV 259 

R„i„. The number of samples riNodes and the nearest neighbor parameter 
k_nearest have to be set to relatively high values compared to wider passages to 
find a way through the narrow passage. 

Planning steps of the local planner are illustrated in figure 4a-c. It can be seen 
that depth first behavior is achieved in the free space, in contrary to the entrance of 
the narrow passage where it takes several steps and the tree widens until the next 
waypoint inside the narrow passage is reached. The safe traversing through the 
narrow passage is only possible since the upward vertical safety maneuver is al- 
ways feasible at that time; it guarantees a recovery of the MAV even if the narrow 
passage is blocked by an unknown obstacle during runtime. The complete trajec- 
tory for the scenario is shown in figure 4c. 

Figure 4d depicts the same scenario with an additional unknown static obstacle 
blocking the path to the second PRM waypoint. It can be seen that the obstacle is 
avoided and as soon as the current_node has reached the goal area of the second 
PRM waypoint, the cost is calculated for the next waypoint and thus, the motion 
plan is optimized ignoring the blocked waypoint. An advantage over pure depth- 
first planners is the usability of the node information stored in the open_Ust. In the 
example, the obstacle is passed on the right side even though a passageway to the 
left was proposed first due to a better value of the partial greedy cost functional J. 

6 Summary and Outlook 

In this work a two-staged motion planner with safety maneuvers based on motion 
primitives has been demonstrated including a straight forward method of motion 
primitive generation for closed-loop fixed-wing MAVs. The introduced frame- 
work is a step towards a computationally inexpensive on-line planner for small 
UAV that is able to function safely in a varying urban environment. A modifica- 
tion of a PRM based planner and a new tree based planner for safe trajectory trav- 
ersing of fixed-wing aircraft were designed and demonstrated in an environment 
that included a narrow passage and the avoidance of an unknown obstacle. 

More detailed parameter studies are being performed for the local planner and 
the interaction of the global planner with the local planner. So far, results show 
good performance, including acceptable runtime behavior. 
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Novel Dynamic Inversion Architecture Design 
for Quadrocopter Control 



Jian Wang, Thomas Bierling, Leonhard Hocht, Florian Holzapfel, 
Sebastian Klose, and Alois Knoll 



Abstract. This paper presents a novel controller architecture for a quadrocopter. A 
two-loop controller using dynamic inversion is designed that allows direct com- 
mands for position and heading angle. The inner loop controls the body-fixed an- 
gular rates. And the outer loop achieves the position control. With this structure, 
the position dynamic equation appears in an elegant form. The derived controller 
is capable of decoupling the strongly coupled dynamics of the quadrocopter, 
maximizing the transmission bandwidth of the position control, as well as elimi- 
nating the singularity caused by the attitude control (i.e. pitch angle at 90 degree). 
Pseudo-control hedging is applied in the position loop to account for limitations, 
saturations, actuator dynamics and delay in the inner loop. The effectiveness of the 
designed controller is demonstrated by an implementation on a quadrocopter 
equipped with an ARM7 onboard processor. 

Nomenclature 

B Body-fixed frame 

W World frame, deduced from NED frame with user-defined x-axis 

Mg^ , M^g Transformation matrices between B frame and W frame 

L,M ,N The moments around x, y and z axis of B frame, respectively 

p, q, r Angular rates around x, y and z axis of B frame, respectively 

[r )^ Position vector denoted in W frame 

V Iw Velocity vector defined w.r.t. W frame denoted in W frame 
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Acceleration vector defined w.r.t. W frame denoted in W frame 

Acceleration vector differentiated w.r.t. W frame 

Gravitational vector in W frame 

Specific force vector in B frame, accelerometer output. 
Total thrust of the quadrocopter 



1 Introduction 

Recent technological progress in low-cost MEMS-based sensors, actuators and en- 
ergy storage devices enables the development of miniature vertical take-off and 
landing (VTOL) systems. The quadrocopter is one of the most preferred types for 
many civil applications as well as research platforms. There are many advantages, 
like easy construction and steering principle, VTOL and hovering ability. However, 
because of the nonlinear dynamic behavior, the control and guidance of these vehi- 
cles is a subject of research, especially in applications such as search and rescue, 
surveillance, inspection, and so on. For these applications, high stability, high pre- 
cision hovering ability, high bandwidth, and high maneuverability are important. 

Previous effort on nonlinear dynamic inversion control for Micro Aerial Vehi- 
cles (MAV) include three-loop design corresponding to inversion of rotational, at- 
titude and path dynamics in separated cascaded loops[l]. A more common con- 
troller architecture is two-loop design [2]-[4], where the outer loop is the position 
control and the inner loop provides attitude control, as illustrated in Fig. 1. Both 
control loops have relative degree 2 dynamics. But they have a limited bandwidth 
and a singularity occurs at a Pitch angle of 90° when Euler angles are used [2]. 
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Fig. 1 Conventional two loops control architecture 



The focus of this work is on the design of a baseline controller using the non- 
linear dynamic inversion method. The system is capable to utilize the high band- 
width of the system and full actuator range without instability caused by actuator 
saturation. It can thus be used as a solid basis to apply augmented control philoso- 
phies, e.g. adaptive control. In addition to the functional requirement, the control 
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algorithm has to be implemented in the embedded hardware and has to fulfill real- 
time requirements while limited memory and onboard processing capacity have to 
be considered. Detailed derivation of the control law design is illustrated, followed 
by the flight test data to verify the design. For the flight test, vision sensors are 
used to aid the inertial sensors embedded onboard. Details about the visual proc- 
essing and the experimental setup can be found in [2]. Specific sensor data fusion, 
trajectory planning and protection mechanisms for takeoff and landing are inte- 
grated for the flight test. 

2 Dynamics of the Quadrocopter 

One well-known inherent quadrocopter characteristics is the strong coupling in 
pitch-yaw-roll. A tradeoff often has to be made between maximization of the sys- 
tem bandwidth and dynamic decoupling. Both problems can be confronted in an 
elegant way if we look at the reaction of the quadrocopter on changes of rotational 
rates of the propellers as follows: 



(0, 








L 

M 










^ 


p 


-•+ 


- 


f 


'I] 




h 


• 


A J 




A) 




A 




.^J 


J' J 


p 


-*- 


r 



JL. " -(C-^4"f (4:"f (ri:f ('n. 



Fig. 2 Signal flow diagram of the quadrocopter dynamics 

As shown in Fig. 2, a change in the rotational rate of each propeller results in 
changes in the thrust of each motor, which gives the change of total thrust and the 
body-fixed rotational moments. These moments result in body fixed angular ac- 
celerations, and consequently the angular rates. Then the angular rates p, q and the 
change in the total thrust yield a change in the acceleration with respect to the 
world frame. The change in acceleration in the end results in change of the posi- 
tion by three integrations. 

The resulting change in acceleration in the body frame consists of 2 integra- 
tions in the XB-ye-plane, whereas in the direction of the ZB-axis acceleration di- 
rectly results from the change in thrust. Different axes of the position dynamics 
are coupled with different dynamic orders. It is difficult to perform exact input- 
output feedback from position to the motor thrusts. A dynamic inversion of the 
position dynamics with relative degree 3 however is possible. 

As can be seen in the signal flow diagram in Fig. 2, attitude angles are not in- 
cluded, but only implicitly appear in the transformation matrix. The inputs for the 
translation dynamics extend to change of acceleration, i.e. angular rate and change 
of thrust. In other words, position can be controlled by a more direct input, which 
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certainly increases the bandwidtii of the position control. Thus the dynamic inver- 
sion of the position dynamics with relative degree 3 is dynamically correct, and 
the complexity of the dynamic inversion is manageable with potential increase in 
the bandwidth. 

For the yaw control, there will be coupling from position dynamics if the azi- 
muth angle is used as the control variable. From Fig. 2, the yaw dynamics, how- 
ever, is not inherently coupled with the translational dynamics. Hence a simple 
way to solve the problem is to control the yaw rate directly, but with an integral 
part in the error controller. Considering the drift rate of the gyro (200° /hour), the 
heading can be well controlled, without coupling with the translational dynamics. 

In summary, the new control structure is an outer loop position control of rela- 
tive degree 3 and an inner loop rate control of relative degree 1 . In the next chap- 
ter, the mathematic derivation for the dynamic inversion is explained in detail. 

3 Dynamic Inversion 

Dynamic inversion is an approach where feedback linearization is applied to the 
outputs of interests. It addresses the problem of controller design by transforming 
a nonlinear system to a linear one by feedback. The transformed plant, as an 
equivalent linear system, may be analyzed by all means of linear system and con- 
trol theory. [5] [6] 

3.1 Inner Loop - Rotational Dynamics 

The inner loop commands angular rates and generates moment commands (m^J 

for the control allocation. The rotational dynamics are well known. By neglecting 
the aerodynamic moment in the moment dynamics, the desired moment command 
is directly obtained by, 

(Ml i = C • {^"' I + {S^""' I X /,«, . (^- 1 (I) 



3.2 Outer Loop-Translation Dynamics 

The command to the outer loop is the desired position in the W-frame, from which 
it generates an angular rate command that is issued to the inner loop. Starting from 
Newton's 2"^ law, we can derive the translational equation of motion by assuming 
that the W-frame can be used as inertial frame. 



m 



■ (5)7 = (f^I + (foIX + (fLX (2) 



For a quadrocopter the aerodynamic lift and side force are negligible and the aero- 
dynamic drag coefficient can be assumed to be constant for simplicity. With these 
assumptions, equation (2) can be rewritten as. 
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Where d^= C^^pS 
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(V)^\\-{V)^ . As the inner loop inputs have not appeared, 



Eqn. (3) is differentiated 



(a)'^ = M^b/b + Mwb/b - dw 
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and the Euler 



differentiation rule can be used. 



M,, = M,, ■ n^' , and Q"^ = skew(df'l 
Hence Eqn. (4) becomes 
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Now the angular rates explicitly appear in the equation. The first order time 
derivative of acceleration is the third order time derivative of the position. So the 
relative degree for position dynamics is three as expected. By the zeros in the first 
two rows of the specific force vector f^ , the yaw rate is algebraically cancelled out. 
The coupled dynamics can be inverted analytically using Eqn. (6). Here we can 
replace the 3"" order time derivative of the position with the pseudo control 
•^www 
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(7) 



A distinguished advantage here is that, the attitude of the quadrocopter (be it Euler 
angle or Quaternion) is not a controlled state. This leads to the dynamic inversion 
equation of such a simple form and does not result in singularities usually caused 
by the attitude. The only singularity in the above inversion is thrust = 0, which can 
be easily remedied in the implementation. Theoretically this control law is capable 
of flying loops, inverted fast descending, etc. 

The overview of the outer loop design is illustrated in Fig. 3, 
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Fig. 3 Overview of the position controller 

4 Implementation of the Two-Loop Design 

The controller was implemented on an ARM 7 processor, using the Simulink 
Quadrocopter Framework [7]. Critical parts of the control system like the inner 
loop, control allocation and data fusion are running with an update rate of 1 kHz 
[2]. With such a fast update rate, control deviation due to the dynamic inversion 
error can be quickly compensated. 

4.1 Quadrocopter Specifics 



The structure of the nonlinear dynamics of the quadrocopter is well known but 
some of the parameters have to be measured, which are listed in Table 1 below. 

' / ' is the arm length between the motor and center of mass. k„ and k^j are 
motor specific parameters and Mp^p is the yaw moment generated by the propel- 



lers , R,, 



M. 



[8]. 



Table 1 Quadrocopter Specific Parameters 



Mass (kg) 


0.68 


Ixx(=Iyy) 


0.007 


kJlO^^N/rpm^) 


5.7 


/(m) 


0.17 


Izz 


0.012 


k„(m) 


0.016 



4.2 Actuator Saturations and Sensor Limitation 



The thrust per motor is in the range of 0.05 N to 3.5 N [8]. Thus the moment gen- 
erated can be calculated easily. To control the moment while keeping the total 
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thrust unchanged, the thrust difference per motor have to be symmetric. Thus the 
control range of each motor around the hover point is about ±1.6N. The maximum 
moment and angular acceleration for each axis are calculated and listed in Table 2. 

Table 2 Actuator Saturations 



M,(=My)(Nm) 


+0.544 


M^(Nm) 


+0.102 


p(=q)(rad/s^) 


±77.7 


r(rad/s^) 


±8.5 



For sensors, the Gyro is limited to a maximum value of 300°/s for the angular 
rates. The accelerometer measures accelerations with a full-scale range of ±1.5g, 
while the operation range is ±3g. The visual tracking system [2] is using a model 
based tracking algorithm with a stereo camera setup consisting of two standard 
webcams. The tracking system has an update rate of 25Hz and a speed limitation 
of about Im /s due to motion blur in the images. The accuracy level varies with 
the lighting situation, the quadrocopter attitude and speed. 

4.3 Reference Model 

The reference model, or command filter is designed to generate smooth trajecto- 
ries which is physically possible for the vehicle to fly. System relative degree, 
actuator dynamics, as well as actuator saturations and sensor limitations are taken 
into account. 

4.3.1 Inner Loop Reference Model 

The moment dynamics have angular rates as outputs and moments as inputs (see 
Eqn. 1). These dynamics have only relative degree one. However, in order to ex- 
plicitly account for motor dynamics, a second order reference model is used in- 
stead of a first order reference model. Hence the second order time derivative can 
be limited. The additional pole can be placed with a small time constant: 0.002s. 

In the experiments [8], the time constants of the propulsion dynamics are found 
to be l/80s for increasing thrust and l/40s for decreasing thrust. A simulation 
model has been constructed to assess and maximize the bandwidth of the inner 
loop, which determines the time constant of the slower pole. 



4.3.2 Outer Loop Reference Model 

For the outer loop, there are three integrations between the inputs (angular rates 
and the change of Thrust) and outputs (position in W frame). Hence a third order 
reference model is used. The time scale separation has to be considered in the 
outer loop. Compared with the attitude control inner loop shown in Fig. 1, the de- 
signed rate control inner loop allows smaller time scale separation, i.e. higher 
bandwidth, for the position control outer loop. In flight test, the increase in the 
bandwidth is not so distinct, as the major limitation comes from the vision system 



268 



J. Wang et al. 



[2]. Nevertheless, the optimal Eigenvalues of the outer loop designed in [2], where 
attitude control was used as inner loop, were at '-4', while the optimal Eigenval- 
ues of the outer loop are now assigned at '-5'. The differential equation of the S^"* 
order reference model is shown below, with a)o=5. 



y'R + 3«oyR + s^oi^R + col- Jr = «o- Jc 



(8) 



4.4 Error Controller 



With exact dynamic inversion the outputs would exactly follow the reference tra- 
jectories. Due to the model uncertainties such as parameter error and sensor error, 
the 3"* order time derivatives of the vehicle position differ from the dedicated 
pseudo control. The effect is propagated through the integration and leads to a dif- 
ference between plant outputs and reference signals. The error controller uses 
feedback of the state errors, augmented by an integral error control to achieve 
steady state accuracy. [9]. 

For the outer loop, the error dynamics is expressed by Eqn. (9), where 

e = yr4-y 
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The error dynamics can be adjusted by the coefficients K, to follow the reference 
dynamics. The integral gain can be determined by pole placement and a 
small value is chosen to ensure steady state accuracy while not affecting the 
performance. 
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4.5 Control Allocation 



For the quadrocopter, the relationship between the forces & moments and the pro- 
pulsion controls are invertible. The desired force and moment commands are de- 
noted with the subscript 'des'. 
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4.6 Pseudo Control Hedging 

Pseudo Control Hedging (PCH) is implemented to 'hide' the actuator dynamics 
from the error dynamics [10]. The expected reaction of the plant V is calculated 
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using Eqn. (6), in which the angular rates are taken from the Gyro measurements, 
the thrust and change of thrust are estimated by means of the modeled actuator 

dynamics Gj^\r,Tj , as they are not measureable. 

v = F(p,q,G^{f,T)) (12) 

Then the hedging signal or the expected reaction deficit can be calculated 

by V;, = V - V . 

The dynamics of the reference model is decelerated by the expected reaction 
deficit. Another function of PCH is to prevent the integrator to wind up in the 
error dynamics [10]. This can be demonstrated in experiment (Fig. 4(e)) by intro- 
ducing an external displacement disturbance to the hovering quadrocopter. 

4. 7 Sensor Data Fusion 

To achieve high bandwidth control, fast data fusion is important. There is not 
enough computation power on the ARM 7 processor for a standard full state Kal- 
man Filter with fast update rate. Instead, a model based Kalman state estimator is 
implemented to fuse the sensor data. The dynamic nature and system orders are 
taken into account in the filter, i.e. third order dynamics in x- and y-axes and 
second order dynamic in z- axis. Hence the inputs are time derivatives of the 
accelerations for x- and y- axes calculated using Eqn. (6) and acceleration in z axis 
obtained from the accelerometer. In total there are 9 states; 3 axes position, 3 axes 
velocities, and accelerations in x- and y- axes and acceleration bias in z axis. Ac- 
celeration bias in x- and y- axes is unobservable as an AHRS (Attitude Heading 
Reference System) filter is used to estimate the quaternions. There are five meas- 
urements, three positions obtained from the visual tracking system and two accel- 
erations in x-y axes from the accelerometer. The constant Kalman gain matrix 
(9x5) is calculated as the optimal Kalman gain for the system offline, given the 
process noise and measurement covariance [11]. 

The vision system uses two standard web cameras to track the quadrocopter, 
based on edge matching algorithms [2]. The measured output is the position vector 
in the World frame at approximately 25Hz with variable accuracy (2cm -10cm). 
The latency of the vision system is about 100ms and it is taken into account by ad- 
justing the position measurement with velocity compensation. 

The most important advantage of the state estimator is that it is running on- 
board with an update rate of 1 kHz. Therefore, disturbances detected by the IMU 
can be compensated within milliseconds. The filter also significantly reduces the 
noise originated from the thermal accelerometer measurements [13]. Overall, the 
filter shows very fast reaction and little noise. 

5 Experimental Results 

In order to show the performance of the new controller, different trajectories like 
circle, infinity sign, and step commands have been tested with data recorded 
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(a) Position Tracking in x axis 
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Fig. 4 Experimental Results of Trajectory Flights 
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online. To show the accuracy of the system, the reference commands and the vi- 
sion sensor measurement are plotted. It can be seen in Fig. 4 (a-d) that the position 
tracking is nearly perfect: the control errors are relatively small (within 5cm) esp. 
in the z axis. The inter-axis coupling is negligible. The robustness and perform- 
ance of the system has been demonstrated in two international trade fairs, 'Em- 
bedded World 2010' and 'ELECTRONICA 2010' held in Germany, where it was 
flying approximately 8 hours every day during the fairs. 

The effect of PCH can be seen in (e) and (f) of Fig. 4. Big overshot caused by 
the integrator wind-up can be compensated by the hedging signals that change the 
reference signals in case of actuator saturations. 

6 Conclusion 

With the current position controller and the vision sensor, the quadrocopter is able 
to fly with good accuracy and a comparatively higher bandwidth. Based on the 
available structure, new control theory and application, like adaptive control and 
advanced data fusion are of interest to the author and to be developed. 
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Parallel Implementation of Constrained 
Nonlinear Model Predictive Controller for an 
FPGA-Based Onboard Flight Computer 



Alexander Joos and Walter Fichter 



Abstract. Model Predictive Control (MPC) is an established control method in 
various application areas. Its ability of taking constraints into account makes it in- 
teresting also for automatic flight control. However, the computational complexity 
of MPC schemes usually limits its application. This paper describes a simple for- 
mulation of a constrained nonlinear MPC (NMPC) approach that can be realized 
on small onboard computers based on Field Programmable Gate Arrays (FPGAs). 
In contrary to classical implementations of MPCs a computationally expensive op- 
timization problem can be avoided while even nonlinear prediction models and 
constraints can be considered. This is accomplished through parallel time-domain 
simulations. To this end, the parallel implementation properties of FPGAs are ex- 
ploited. The 3d-kinematics is proposed as prediction model for the NMPC to plan 
the aircraft state trajectory (position and attitude) taking constraints and obstacles 
into account. Simulation results with a nonlinear 6 degree of freedom simulation 
model verify the functionality. Feasibility of hardware synthesis of parallel pre- 
dicted models for the NMPC approach on an FPGA is shown by analysis. 

1 Introduction 

Flight safety is an important topic for manned aircraft as well as for unmanned 
aerial vehicles (UAVs). Thus there is a need for automatic aircraft control 
algorithms that are able to cope with constraints in the aircraft states, inputs and 
with obstacles incorporating nonlinear dynamics of an aircraft. One possible ap- 
proach to solve such problems is Nonlinear Model Predictive Control (NMPC) 
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with constraints. NMPCs have the abihty to generate optimal inputs for nonhnear 
plants that can not sufficiently be controlled by MFCs with linear prediction mod- 
els under the consideration of constraints. 

The basic idea behind NMPC is to solve repeatedly an optimal control problem 
with finite horizon. This means that an optimal control input is to be found that re- 
duces a cost function in which deviations from a commanded state are weighted 
while considering constraints of the inputs and of the plant. This powerful control 
algorithm is therefore a good candidate to control unmanned aerial vehicles 
(UAVs) while considering nonlinear aspects of the plant and constraints on inputs 
and the states. On the other hand NMPC requires strong computing power to solve 
nonlinear constrained optimization problems in real-time, which makes this ap- 
proach hard to implement on small computers as they are usually used as onboard 
flight control computers for small UAVs. 

In literature, some references can be found that cope with the implementation 
of MPCs on small computers with moderate computing power. Reference [1] 
demonstrates the implementation of a constrained MPC on an FPGA that uses a 
linear prediction model. For optimization the interior point method with dense ma- 
trix formulation is used to solve the quadratic programming problem. A linear air- 
craft model with 4 states and 1 input is used to demonstrate the control of the alti- 
tude by the use of the elevator. The ability of the FPGA to compute code in 
parallel is not used. Parallel implementation on an FPGA is used in Ref. [2], but it 
is restricted to linear systems with constraints. In Ref. [3] an implementation of an 
explicit MPC is described in which the optimization problem is precomputed and 
the real-time control problem is simply reduced to the evaluation of a piecewise 
linear function. The limitation of this approach is that the memory requirements 
increase rapidly with an increase of the problems dimensions. An algorithm spe- 
cific processor for embedded unconstrained MPC is proposed in Ref. [4]. The pre- 
diction model in the demonstrated application is a linear state space model. For 
solving the optimization problem Newton's Method is used. Matrix operations are 
computed in an auxiliary unit that acts as a matrix coprocessor. Results are pre- 
sented from controlling the linear model of a rotating antenna with 2 states and 1 
input, constraints are not considered. In Ref. [5] the application to a nonlinear glu- 
cose regulation control problem with constraints on the input is demonstrated by 
HIL simulations. The nonlinear model is linearized for several intervals to gener- 
ate multiple linear state space matrices for use in the MPC algorithm and discre- 
tized with a sample rate of 5 min, which is far too large for flight applications. The 
application of a nonlinear MPC for high level control for a fixed wing UAV is de- 
scribed in Ref. [6]. The control problem uses an error dynamics model with 2 
states and 1 input that bases on the 2d-kinematics in horizontal plane. The input is 
constrained. The high level control is implemented in a PC 104 onboard flight 
computer and tested in HIL simulations as well as with MATLAB simulations. 
Although the PC 104 is a relatively powerful onboard computer (comparable to a 
desktop PC) and the prediction model is of small dimension, 4 Hz update rate is 
the limit that is reported in this reference. 
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In summary, online computed MFCs on small computers are restricted to 
low-dimensional problems with linear prediction models. Then, the optimization 
problem for linear MFC is the solution of a quadratic program that can be solved 
efficiently even online [7, 8]. In case of NMFC applications [6] powerful and rela- 
tively large and heavy computers are used together with models of low complexity 
(2d-kinematics). Thus, it is not applicable for small fixed wing aircraft. 

Reference [9] describes an optimization method based on brute force method 
for NMPC that does not suffer from complex optimization. In general this optimi- 
zation method is also computationally very expensive since numerous prediction 
models representing the plant have to be propagated with numerous input candi- 
dates in each control step. No implementations on small flight control computers 
for UAV application are reported in literature. 

In this paper, the design of a constrained NMFC based on optimization with 
time-domain simulations is presented. It is shown that a UAV state trajectory con- 
trol problem can be solved even with a low number of models that have to be pre- 
dicted. Furthermore a partially parallel implementation of the prediction models is 
proposed to achieve feasibility. 

The paper is structured as follows. After a review of classical MFC formula- 
tions in chapter 2, a parallel implementable approach is introduced. As prediction 
model for high level state trajectory planning for the aircraft the 3d-kinematics 
model is proposed. A method to refine the commands of the NMFC without addi- 
tional computational effort follows. Simulation results of a nonlinear 6 degrees of 
freedom (6-DoF) simulation model controlled by a constrained NMFC with the 
nonlinear 3d-kinematics prediction model are demonstrated in applications with 
collision avoidance and a kind of unconventional landing scenario. Both scenarios 
require a state trajectory planning by the NMFC. The last chapter presents results 
from hardware synthesis of parallel prediction models for an FFGA. 

The first contribution of this paper is a feasibility proof of a NMFC based on 
time-domain simulation optimization, applied to a UAV state trajectory planning 
problem even with a low number of prediction models. The computationally most 
expensive part of this kind of constrained NMFC formulation is the real-time 
model prediction. Therefore, a second contribution is a parallel implementation 
scheme of model prediction for a constrained NMFC on a FFGA-based onboard 
flight control computer as it is presented e.g. in Ref. [10]. This enables the appli- 
cation of constrained NMFC to small flight vehicles. 

2 Classic Nonlinear Model Predictive Control Formulation 

The optimization problem for a NMFC can be described by 

miny(u) = min \ F(x(T),u(T))dT (1) 

u u J 



276 A. Joos and W. Fichter 

subject to 

JL = f(x(T)MT)), Xo=x(fo) (2) 

and with equality and inequality constraints gi and gj. 

gi(x(r),u(r)) = 0, g2(x(r),u(r))>0 Vre [?o,fo +7'^] (3) 

X and u are predicted states and inputs respectively, /(u) is the cost function 
that shall be minimized with respect to u , and T is the prediction horizon. 

NMPC requires a computationally expensive online solution for a nonlinear op- 
timization problem [7]. Reference [7] names three approaches from which only di- 
rect solutions using a finite parameterization of the controls and/or constraints are 
normally applicable for on-line application. A direct solution for this problem can 
be achieved by parameterization of the controls at every predicted sampling inter- 
val and holding them constant for that interval [7]. To calculate the cost function 
the systems dynamics described by equation 2 are solved by numerical integration. 
Equality and inequality constraints are evaluated with the predicted inputs and 
states as well. The resulting optimization problem can then be solved with sequen- 
tial quadratic programming [11]. The matrices for this approach are often dense, 
which makes the solution with this approach computationally expensive. The "si- 
multaneous approach" in Ref. [7] can fail when the optimization cannot be com- 
pleted in time. In this case, feasibility is not guaranteed with this approach. 

All of the above methods have the disadvantages of being computationally 
extensive or even unsolvable on computers with low computational power. There- 
fore in the following section, a simpler approach to the solution and on-line 
implementation of NMPC problems is presented. 

3 NMPC Approach with Parallel Implementation 

3. 1 General Description of the Proposed NMPC Approach 

The idea is to propagate a nonlinear simulation model with a certain set of combi- 
nations of control input candidates [9] that are quantized in amplitude and discre- 
tized in time and to evaluate the resulting state vectors and inputs in a cost function. 
In this paper, to save computational cost, the prediction of the models is proposed 
to be done partially in parallel on an FPGA which is especially suited for this task. 
The optimization problem is therefore reduced to finding the cost function with the 
smallest value and taking the corresponding input of the first propagation step as 
the optimal control input. 

The parameters of two NMPC setups demonstrated in this paper are listed in 
Table 1. To introduce the idea, NMPC 1 will be considered in the following. Later 
in this document results will be shown also with NMPC 2. 
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Table 1 Parameters of the proposed NMPC used for simulation. 



Parameters of the proposed approach 



NMPCl 



NMPC 2 



Prediction Horizon 

Number of variable control inputs 

Time intervals with constant control input 

Number of discrete candidates per control input 

Discrete candidates for each control input 
in the first time interval with constant control 
input (discrete candidates in the second time 
interval are chosen =15% smaller) 

Prediction steps in each of the m time intervals 

Number of predicted models 

Number of resulting predicted state vectors 

Number of cost functions to evaluate 

Number of model prediction steps in total 

Constraints in attitude 

Sample rate of the NMPC controller 



r = 7s 

n = 2 ip^, q) 

m = 2 (3s, 4s) 

(1 = 2, 

p^ = (/^,-l)-0.1rad/s; 

i„ = 0' 1.2 
i = (/■,-0.8)-0.07 
rad/s;i, = 0, 1,2 
h = 3,h,= 4 
d^n^l+d^n^m = 90 
d'^n'^m = i,\ 

= 351 

l^l<30M6'l<15° 
10 Hz 



r =7s 

n = 2 ip^, q) 

m = l (3s, 4s) 

d = 9 

p.= (;^_-4)-0.03rad/s; 

;; = 0,...,8 

q^ = (/;-3.9)-0.02 rad/s; 

;; = o,'..,8 

h = 3,h=4 

d'^n'^l+d^n'^m = 6642 

d''n''m = 6561 

d''n''m = 6561 

h^d''n''l+ h^d^n^m 
= 26487 

l^l<3OM0l<15° 

10 Hz 



The prediction model of the NMPC is the nonlinear 3 dimensional kinematics 
model with prediction starting condition Xg = x(/q ) where x(/o ) is the actual 
state (position and attitude) of the aircraft (s = sin, c = cos): 
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Inputs to the prediction model u are bodyfixed velocities {u,v,w) and turn rates 
(p,q,rf, the predicted states x are euler angles {(p,6,y/f to describe the attitude of 
the aircraft, and earthfixed "north-east-down" position {x,y,zf. Inputs m, v, w, r are 
chosen as constants in the demonstrated results in this paper. Each of the n = 2 
variable inputs (roll and pitch rate) can take d = 3 discrete candidate values 
(Pc, '?c)^per time interval with constant discrete value (NMPC 1). 

The number of predicted models over a first time interval is d" = 9, each with 
one of nine constant combinations of input values pc and qc- The resulting nine 
state vectors of these nine predicted models for the first time interval are used 
as start conditions for another time period in which again the input values can take 
d" = 9 combinations of discrete values. In the second time period all nine combi- 
nations of input values are used with all nine resulting predicted state vectors of 
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the first time period. This leads to d"'" = 81 predicted state vectors after the sec- 
ond time interval. Figure la) shows the predicted positions of the 81 predicted 
state vectors as they are calculated in one single NMPC update. Figure lb) shows 
the NMPC update with 1 Hz NMPC update rate. In the simulation results in this 
paper the NMPC update rate is 10 Hz, so in the results shown later in this paper 
there are 10 times more predicted state vectors than displayed in Fig. lb). 




Fig. 1 a) Predicted positions in an example with 81 predicted state vectors (position and at- 
titude) according to Table 1 , NMPC 1 . b) Prediction trees calculated with an NMPC update 
rate of 1 Hz. 



A cost function J is evaluated with all d " " resulting state vectors and corre- 
sponding inputs: 
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where / = fo, to+St,..., to+Tp and Tp is the prediction horizon, St is the length of a 
time interval with constant value of control input in the numerical integration of 
the dynamic prediction model. Xjet is a commanded final state for the aircraft 
(position and attitude). To reach a commanded point with a commanded attitude, 
deviations of the predicted state (position and attitude) from Xret and predicted in- 
puts are weighted in F. Approaching the reference point from a certain direction is 
fulfilled by appropriate weights for x- and y- direction and a weight for a deviation 
between the actual heading of the aircraft and the direction to the commanded fi- 
nal position. Constraints c^ cg, Cp for roll angle, pitch angle and position are con- 
sidered as barrier functions: 



A: , > if constraint violated 
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(6) 



Constraint Cp is used e.g. for obstacle avoidance and positions below ground level. 
Avoiding the obstacle is supported by a cost that increases when the distance dist 
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between predicted position and the obstacle decreases. The optimization problem 
in this case is reduced to finding the one cost function /„,!„ with the lowest value 
out of the 81 resulting cost functions (Eq. 7). The optimal input is the predicted 
control input of the first time interval that belongs to cost function /„,!„. 



mm 



{y./Z^eN Al<fe<^«'«}=/^„ (7) 



This input is applied to the plant for one control cycle and the procedure is re- 
peated in every control cycle as it is usually done in MPC approaches (Fig. lb). 

3.2 Refinement ofNMPC Commands 

In this chapter an idea is presented how to generate finer and therefore more pre- 
cise NMPC commands without increase of computational effort. One possibility is 
to use more and finer discretized candidates as in NMPC 2 (Table 1) what leads to 
an increase in computational cost. Another idea is to use the same number of can- 
didates, but finer candidates for p and q in the first of the m time intervals in the 
NMPC and add these to the actual roll rate p(to) and pitch rate q{to) of the plant. 
The model prediction tree (Fig. 1) is therefore being computed starting from the 
actual position, attitude, roll and pitch rate of the aircraft. 

p, =(j„-l)-0.03+/7(fo) j„ =0,1,2 
^,=(j,- 0.8) -0.021 + ^(^0) 7, =0,1,2 

The presented approach introduced in Chapter 3 reduces the computational effort 
significantly by parallel implementation of prediction models (this can be done in 
parallel in each of the m time intervals) and if a low number of discrete input can- 
didates are used. Of course, the choice of number of discrete values each control 
input can take is a trade-off between computational effort and optimality of the so- 
lution. Simulation results will show that even low numbers of input candidates can 
be used due to the repeated computation of optimal controls within the NMPC 
scheme with high update rate. 

4 Simulations Results for UAV State Trajectory Planning 

4.1 Scenarios and Control Structure 

At first a scenario is presented in which the aircraft starts from an arbitrary posi- 
tion and attitude and shall reach a given setpoint of position and attitude while 
avoiding an obstacle without violating constraints. This requires some kind of 
state trajectory planning which is solved in this paper by NMPC. For this scenario, 
results with high and low numbers of discrete input candidates in the prediction 
model are demonstrated. In a second scenario a kind of unconventional landing is 
demonstrated that also requires state trajectory planning due to the initial starting 
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state relative to the commanded final state. In both scenarios no steady state condi- 
tions have to be fulfilled and no planned state trajectory is required a priori. The 
NMPC controlled aircraft has to find a feasible state trajectory to a commanded 
final state (position and attitude) without violating constraints and in scenario 1 
while avoiding an obstacle automatically. 

The control structure consists of two loops. An inner Linear Quadratic Regula- 
tor (LQR) control loop (low level controller) commands aileron, elevator, rudder 
and thrust. It is designed as a proportional/integral LQR controller. Since the air- 
craft has only 4 control inputs, only the 4 aircraft states: bodyfixed rates p, q and 
the bodyfixed velocities u and v can be controlled exactly by the low level control- 
ler. Bodyfixed down- velocity w follows from the pitch stability of the aircraft and 
varies only slightly around a constant value. Yaw rate r follows from the dynamics 
and the kinematics of the aircraft, e.g. r = g/V sin((Z>) in case of a steady state coor- 
dinated turn, with g = gravitational acceleration and V = speed of the aircraft [12]. 



Commanded final state 

Constraints 

Obstacles 

> 



NMPC with 3d- 

l<inematics- 

model 



= 20m/s 



1 com 
Hcom 



0ml s j-^ Airplane with low level 
LQR controller for 
(u,v,p,q) 



y 



wf 



Fig. 2 Control structure of the demonstrated example. 

The outer loop (high level controller) is a NMPC controller that generates com- 
mands for pitch and roll rate (pcom, Icomf according to the methodology introduced 
in the previous chapter. 

Simulations in which the 3d-kinematics in the NMPC was predicted with r = 
rad/s and with r = g/V sin((^) have shown both comparable control results. In the 
following, results will be shown with prediction input r = rad/s. Although the 
prediction error is higher than with r = g/V sin(^), the control problem can be 
solved due to the high update rate (10 Hz) of the high level NMPC controller that 
generates changing commands (pcom, Immf ^'^^^Y 0.1s. 

With this control structure the 3d-kinematics model can be used as adequate 
prediction model for the nonlinear MPC as will be demonstrated in the following. 

4.2 Simulation Results 



In this chapter simulation results are demonstrated with the NMPC approach 
as proposed in chapter 3.1. For simulation purposes a nonlinear 6-DoF aircraft 
simulation model is used. The states of the 6-DoF simulation model are bodyfixed 
velocities (M,v,w)"'^and turn rates {p,q,rf, Euler angles {(p,6,y/f to describe the atti- 
tude of the aircraft, and earthfixed "north-east-down" position {x,y,zf- The aircraft 
dynamics in the simulation are modeled with aerodynamic force and moment co- 
efficients gathered from USAF Digital DATCOM, gravity forces, control forces 



Parallel Implementation of Constrained Nonlinear Model Predictive Controller 



281 



due to control surface deflection that are also generated with DATCOM, and pro- 
peller forces. 

Table 1 lists the parameters of two NMPCs used for the following simulation 
results. In the first scenario (Fig. 3) the aircraft starts from an initial position 
(x = Om, y = Om, z = -lOOm)^ in earthfixed "north-east-down" coordinates with 
(j)= \l/= 0°, 9= 3°. The commanded final state is x^t = (x = -1000m, y = -1000m, z 
= Om, ^=0°, d= 3°, ^= ± 90°)^ where if/= ± 90° is e.g. the heading of a runway. 
Figure 3 shows 3d-plots of NMPC 1 and NMPC 2 (Table 1) apphed to a collision 
avoidance problem. 



<2DD 




-1000 



■800 



y, m 



Fig. 3 Scenario 1: 3d-plots of NMPC 1 (81 predicted state vectors/controller update) and 
NMPC 2 (6561 predicted state vectors/controller update) both with and without collision 
avoidance. 



Throughout the flight, constraints in roll and pitch angle shall not be violated 
which is fulfilled as can be seen in Fig. 4 for NMPC 1. During the first turn the 
NMPC controlled aircraft uses the maximal allowed angles in order to change the 
direction as fast as possible. The obstacle has been placed right in the obstacle-free 
path (curves that go through the cylinder in Fig. 3). With Fig. 3 it can be stated, 
that the results of NMPC 1 and NMPC 2 are comparable. The planning and obsta- 
cle avoidance problem is solved in either case. 

Both controllers prevent a violation of the constraints (results for NMPC 1 in 
Fig. 4) and both manage to avoid the obstacle (Fig. 3). Remember that NMPC 2 
uses a higher number of discrete values for each control input (factor 3, Table 1) 
and a far higher number of predicted models per controller update than NMPC 1 
(factor =; 74, Table 1). 
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Fig. 4 States of tlie aircraft during fligiit according to Fig. 3 witli NMPC 1 applied. Con- 
straints are plotted as dashed lines. 

The trade-off between optimality and computational expense is won by NMPC 
1. This has been observed in numerous simulations in which NMPC 1 has shown 
that it can solve control problems even with this low number of discrete candi- 
dates for each control inputs. The reason for this is the repeated optimization 
scheme of the NMPC at an update rate of 10 Hz. The prediction tree with the 81 
predicted state vectors is calculated every 0.1s and the commands for the turn rates 
are updated with the same rate. The control commands of the NMPC are displayed 
in Fig. 5. It can be seen that the commands for roll and pitch rate are well con- 
trolled by the low level controller even if the commanded rate by the NMPC 
changes quickly (e.g. q at about 2.6 s). Due to this fast update in commands from 
the NMPC it is possible to command even little changes in the attitude. 




27 28 
time, s 

Fig. 5 Roll rate and pitch rate commanded by NMPC 1 and controlled by the LQR low 
level controller according to the scenario 1 in Fig. 3. 



In a second scenario the aircraft starts from an initial position (x = Om, y = Om, 
z = -ISOm)"'^ with i/>= y/= 0°, 0= 3°. The task is to reach a final state Xjet = (x = 
10m, y = 30m, z = Om, (p=0°, 6= 3°, i//=0/180°f which lies below the initial 
position, while taking constraints 1^1 < 30°, \0\ < 20° into account. With these 
constraints there is no straight path the aircraft could fly to reach the commanded 
final state. The NMPC finds a feasible state trajectory to accomplish the reference 
state Xfef. Of course, since this is an optimization problem with numerous reference 
states (final position, final attitude) and constraints, not all states can always be 
controlled exactly in this kind of landing scenarios. 
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Fig. 6 Scenario 2 with final position right below the initial position. 

4.3 Results for Refined NMPC Commands 

Figure 7 shows the commands of the same scenario as in Fig. 3 but with far 
smoother commands for p and q than in Fig. 5 (method as described in chapter 
3.2, Eq. 8). As in Fig. 1 (NMPC 1, Table 1), 81 resulting state vectors are pre- 
dicted and evaluated in cost functions every 0.1s. Simulation results are compara- 
ble to the results shown in Fig. 3 with less gain in altitude during the first turn and 
smoother behavior in pitch 0. 
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Controlled 
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10 20 30 
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Fig. 7 Refined roll rate and pitch rate commanded by the NMPC and controlled by the LQR 
low level controller according to a scenario as in Fig. 3. 



5 Hardware Synthesis of Parallel Model Prediction for an 
FPGA 



Model prediction is the computationally most expensive part of the presented con- 
strained NMPC approach. To this end, in this section feasibility of parallel model 
prediction on an FPGA for the constrained NMPC problem is demonstrated with 
the nonlinear 3d-kinematics prediction model as it is used in the previous chapter. 
Trigonometric functions are calculated through interpolation of look-up-tables for 
cosine and its inverse to enable parallelization. 

In the following, required resources of an FPGA are reported for several con- 
figurations of parallel computed nonlinear 3d-kinematics models (Table 2). The 
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code has been implemented in the DK Design Suite of Mentor Graphics that al- 
lows the programming of an FPGA in a C-like language called Handel-C. The 
presented computational effort results are generated in the DK Design Suite. The 
FPGA for which the code is built is a Xilinx Spartan 3A DSP. 

FPGAs contain configurable logic blocks (CLBs) to synthesize the program 
code in hardware. Each CLB contains slices and each slice is a group of logic 
blocks such as look-up-tables (LUTs) and flip-flops (FFs) [13]. To save logic 
blocks, Arithmetic Logical Units (ALUs) can be used by the compiler e.g. for 
multiplication. In the following the number of occupied slices will be regarded as 
indicator for hardware effort of the program code. 

It can be stated, that with an increasing number of parallel implemented predic- 
tion models, the number of occupied slices on the FPGA increases approximately 
linearly (compare cases 1, 5, 7 in Table 2). If more than one prediction step per 
parallel implemented prediction model is calculated (this is done in series on the 
FPGA) more slices are required (compare case 2 against case 1) and the total 
computing time increases due to serial computations. 

Table 2 Comparison of required resources of the FPGA with different implementations of 
model prediction. 
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For a simple estimation of maximal number of model prediction steps that can 
be computed on the FPGA in one second it is assumed that no other code is run- 
ning on the FPGA, so case 7 can be considered in which 70% of the FPGA-slices 
are used. Further assumptions are, that each parallel prediction model consists of 
approximately 100 lines of code and that 81 model prediction steps can be calcu- 
lated in parallel (case 7 in Table 2). The clock-rate of the FPGA for this estimation 
is calculated as l/(time delay of the longest path). 

Thus the number of model prediction steps that can be calculated in one second 
on the FPGA is: 
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clockrateparallelmodelpredictionsteps _ modelpredictionsteps 
linesof codepermodelpredictionstep s (g\ 

^- 5 ^-81 » 3.2-lO'i 

25.44-10"'.5 100 s 

Computation time T for 81 parallel model prediction steps with each 100 lines of 
code is: T= 100-25.44-10"'s/81 = 3.1-10"**s. Since communication aspects between 
the FPGA and a serial computing unit on the onboard computer and the calcula- 
tion and evaluation of a cost function are not regarded in this simple estimation the 
calculated number of maximal model prediction steps/s is just an upper bound that 
cannot be reached in a full NMPC implementation. It is expected that the maximal 
number of computable model prediction steps will be reduced by a factor of 
0.1-0.01 in relation to the value in equation 9. This is still significantly more than 
the number of model prediction steps used in the simulations above that verified 
feasibility of the proposed approach (Table 1, Fig. 3-9). 

6 Conclusion 

This paper demonstrates the feasibility of a proposed parallel implementable con- 
strained NMPC approach to a UAV state trajectory planning problem with and 
without collision avoidance. The presented approach is based on time-domain 
optimization which requires the prediction of time-domain models. A method to 
refine the NMPC commands without additional computational effort has been in- 
troduced. The functional feasibility of the constrained NMPC with 3d-kinematics 
as prediction model even with a low number of control candidates has been 
shown in simulations. To save computational cost, parallel implementation of 
prediction models on an FPGA has been proposed and implementation feasibility 
has been shown through hardware synthesis of different configurations of parallel 
prediction models for an FPGA. 

The presented constrained NMPC approach enables the use of constrained 
NMPC with dimensions that were hardly implementable on small UAVs before. 
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Robust Linear-Parameter Varying Autopilot 
Design for a Tail/Thrust Vector Controlled 
Missile 
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Abstract. A robust autopilot design methodology using linear parameter varying 
transformations is presented and applied to a high-agile surface launched air de- 
fence missile, which is currently developed by Diehl-BGT-Defence. The lateral 
dynamics of the tail/thrust vector controlled missile are modelled as a second- 
order quasi-linear parameter varying (LPV) system. The incidence angle is used as 
exogenous variable, which is assumed to be estimated during missile flight. De- 
coupled lateral dynamics are assumed because of the application of a bank-to-turn 
manoeuvre plane angle control approach. Lateral single channel flight controllers 
are designed via Hoo-optimal control and p-synthesis with the LPV lateral dynam- 
ics, which are extended by uncertain models of control actuating system, time- 
delay and body bending model. The flight controllers for lateral dynamics are 
designed at a number of operating points described by the LPV model over the 
MACH flight envelope. The controllers are implemented using a gain scheduling 
approach, where an altitude dependent gain loss in the control loop is compen- 
sated with the inverse normalised air density. The flight controllers were imple- 
mented in the nonlinear simulation environment and tested in extreme flight 
manoeuvres. All flight controllers showed good damping and acceleration tracking 
performance and were stable during nonlinear simulations. 

1 Introduction 

Flight control systems for modern missiles have to face greater challenges in terms 
of control system design. The missile autopilot has to be designed for a large flight 
envelope of the missile. For high-agile missiles, this means high incidence ma- 
noeuvres, mainly at the beginning and during the endgame of flight. In addition to 
demanded large lateral acceleration capacity, the flight control system has to be 
designed to engage manoeuvring targets over an extended velocity and altitude 
range. The resulting multivariable system to be controlled is highly nonlinear and 
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time-varying. The autopilot has to guarantee feedback control properties like fast 
and accurate acceleration reference tracking, good rate damping and robust stability 
and performance. 

Traditionally, missile autopilot design is done using gain scheduling control 
[1], [2]. The nonlinear, time-varying system is linearised using a first-order Taylor 
approximation at a so called trim condition, which is a quasi-equilibrium, obtained 
at a certain time of flight. Classical linear controller design techniques can then be 
used for the resulting linear time-invariant system. This procedure is usually re- 
peated for a family of operating points, for example Mach number, dynamic pres- 
sure, incidence angle, motor burn time and manoeuvre plane angle. The resulting 
controller parameters are interpolated depending on the current operating condi- 
tions during missile flight. This approach worked well in most applications, but 
problems with a rapidly changing incidence angle, which leads to a fast parameter 
scheduling variable in the frequency range of the incidence angle mode, may oc- 
cur [3]. A further disadvantage of this approach is the rising number of controller 
design points resulting from the extended flight envelope of modern missiles. This 
leads to a higher development cost, or demands the introduction of automated tun- 
ing methods [4]. 

With the introduction of robust linear control design techniques, new possibili- 
ties for missile autopilot design were given. The definition of certain require- 
ments, as well as uncertainties to the design process is possible with methods like 
Ha,-optimal control and ^-synthesis. As a result of the optimisation process, a con- 
troller is given, which guarantees robustness and performance properties. The ap- 
plication of robust control techniques to missile autopilot design showed promis- 
ing results [5]-[9], although more than one robust controller is needed to cover the 
flight envelope of the missile. This problem can be overcome with schedul- 
ing/blending techniques [10] and was successfully demonstrated for the industrial 
missile autopilot example in [9]. The autopilot of a high-agile tail/thrust vector 
control air-to-air missile of this industrial application was designed for thrust/ 
burnout phase, varying altitude and incidence angles at a given dynamic pressure 
operating point. In addition to the successfully demonstrated controller blending, 
flight controller anti-windup was proven to work reliably during real world flight 
tests [11]. The approach of [9] was further developed in a missile control approach 
for a high-agile tail/thrust vector control ground-to-air missile, with industrial ap- 
plication [12]. In this approach, the robust controllers are designed at certain Mach 
operating points and gain scheduled over altitude. Uncertainties included in the 
approach are parametric airframe uncertainties resulting from linearisation over 
the incidence angle range and at different motor burn times. This approach worked 
well for control performance tests using a flight test validated nonlinear six- 
degrees of freedom model. Since the nonlinear lateral dynamics, depending on the 
incidence angle are modelled as uncertainties, the control design approach is con- 
sidered suboptimal in performance terms. 

To include the nonlinear incidence angle dependent lateral dynamics to a linear 
time-invariant controller design, linear parameter varying (LPV) control methods 
can be used [3]. LPV-based methods consist of reformulating the nonlinear plant 
to a linear time-varying model for which then a linear controller design technique 
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can be applied [3], [13]. In the approach presented in [3], the nonlinear lateral dy- 
namics are brought to a quasi-LPV form by a state transformation. The approach 
of [3] is adopted in this paper and revised. The nonlinear lateral dynamics are re- 
formulated for quasi-LPV transformation and augmented by uncertainties in Sec- 
tion 2. In Section 3 the controller design is presented via (i-synthesis. Different to 
[3], the controller is designed as a (1, 2) controller for acceleration tracking and 
yaw/pitch rate damping. Implementation aspects are presented in Section 4. The 
controller is tested in simulations with the nonlinear lateral missile model and 
finally in a nonlinear validated 6D0F model in Section 5. In addition to the two 
lateral LPV channel controllers a roll controller is used to damp the roll rate and to 
control the manoeuvre plane. Finally, this paper ends with results and a discussion 
in Section 6. 



2 System Model 



2.1 Lateral Dynamics 



The lateral yaw dynamics at a certain operating point can be described by the sec- 
ond order nonlinear state-space model of the form 
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where ^ is the sideslip-angle, r is the yaw rate, m is the missile mass, lyy is the 
missile inertia, ^ the rudder position given by the control actuating system (CAS) 
and Vq is the absolute velocity. Note that the output equation for a,, ^.g is nonline- 
arly dependent on j8 . This term is linearised and regarded as uncertainty in the 
control design approach. The force and moment terms b ,b^ in the control input 
matrix are linearised at low sideslip-angles 
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and contain aerodynamic and thrust-vector control (TVC) derivative values, with 
q , the dynamic pressure, S^^f , D^^j , the aerodynamic reference area and diame- 
ter, respectively. Similar to by,b„, the term Nr is the aerodynamic yaw moment 



^ref^ref ^ref 3c„ 



Va 



Note that by,b„ 



derivative with respect to the yaw rate N^ =q 

are nonlinear with respect to the sideslip-angle and time varying with respect to 
TVC. This effect is neglected in the quasi-LPV transformation but specified as pa- 
rametric uncertainty in the controller design. The nonlinear aerodynamic force and 
moment entries in (1) are generated from aerodynamic data at each Mach design 
point as table interpolation data 

Y{l5) = ~qS,,fCy{fi) 

The model of the lateral dynamics (1) is given at each Mach number of the flight 
envelope of the missile for the controller design grid. All model data is generated 
at sea level altitude and stored for controller design. The equation of (1) is ex- 
tended by a linear CAS model, described by a second order differential equation 
with uncertain damping /^cas ^^^ Eigenfrequency COq^^ 
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For controller design and test, the model of (1) is extended by a model of the 
structural vibrations (body bending). Only the first bending mode is modelled us- 
ing a low damped oscillator state space model of second order 
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in which ffij,,^ and ^^^^ are the body bending frequency and the damping. The in- 
put ^ is the rudder command, calculated from fin deflections and the outputs are 
the delta rates and accelerations, applied by addition to equation (1). ^^ ,,^1, and 
k iji, are the rate and acceleration gains, calculated from constants, derived by a 
mechanical finite element model and validated by body bending tests [9]. Finally, 
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the time-delay of the flight computer was modelled as two sample time instances, 
with a continuous first order Pade approximation to 



Gr,a(s)- 



T 

-^^ + 2 
T 



(5) 



where T^ is the system sampling time, n^ =2 is the assumed time delay. Equation (5) 

is applied to the outputs of (1). The flight controller model, consisting of equations 
(1) to (5) is given in Fig. 1. 

2.2 LPV System 

The nonlinear system of lateral dynamics that is considered here shall be trans- 
formed to a linear system, whose parameter matrices A, B and C depend on an 
exogenous time-varying variable 



x = A(0)x + B(0)u 



(6) 



with input vector u, state vector x and output vector y. Note that the exogenous 
time-varying parameter of the quasi-LPV system form (6) is actually not exoge- 
nous, but the internal angle-of-attack state and is assumed to be estimated in the 
flight control system. The nonlinear system considered here is an output-nonlinear 
square system with state dependent nonlinear matrix f(z), linear matrix A(z), B(z) 
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y = Cz 

which is to be transformed to the quasi-LPV system 
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Fig. 1 Model of the lateral dynamics, including control actuating system (CAS), structural 
dynamics (body bending) and time-delay of the flight computer 
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y = Cz 



with the transformed input vector u* and state vector w*. The requirement for the 
transformation from (7) to (8) is f (0) = and that there exist nonlinear continu- 
ous, differentiable functions Wg„(z),u^„(z) for which holds 



= f(z) + A(z) 
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Using (9) and the introduction of a new state w = w- Wg„(z) and an input trans- 
formation u =u-Ug„(z), equation (7) can be transformed to the quasi-LPV 
system. For the transformation the matrices A(z),B(z) are rewritten to 
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After some algebraic reformulation, the quasi-LPV system is obtained as 
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Finally, for the nonlinear yaw dynamics of (1) the quasi-LPV model is given by 
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3 Controller Design 

To conduct a controller design the quasi-LPV model (12) has to be modelled as a 
linear time-invariant (LTI) system. Therefore, the sideslip-angle /i is assumed to 
be constant. The linear model is obtained at a certain sideslip-angle and can be 
used for controller design. To include the uncertainties of the varying sideslip- 
angle into the controller design, the parameters for varying incidence angle are de- 
termined over the incidence angle range and modelled as parametric uncertainty. 
As a nominal value the zero sideslip-angle parameters are used. In addition to 
the uncertainty, modelled to sideslip-angle dependent parameters, parametric un- 
certainty was assumed to the control input matrix parameters by, b„. As mentioned 
before, this is done to include incidence angle dependent variation in the control 
efficiency. Note that this effect is reduced with an active TVC, but has to be taken 
into account. The plant of lateral dynamics is augmented with equations (2)-(5). 




[w^^]-^* 



Fig. 2 Plant interconnection structure with linear time-invariant lateral missile dynamics for 
controller synthesis 
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No body bending model was included in the controller design. The augmented 
model with weighting function for controller synthesis is shown in Fig. 2. From 
Fig. 2 it can be seen that the controller design is conducted as a (1, 2) controller, 
consisting of a rate feedback and an acceleration tracking loop. 

The controller output u (input to the actuator) is fitted with a complex weight- 
ing to limit the actuation bandwidth 

^M = '-^^^ (15) 

Pm^ + I 

where fc„ is the static gain, z„ is zero and /?„ is the pole of the transfer function. Pa- 
rameters for (15) are determined for each Mach controller design point from 
crossover frequency and gain at high/low frequency. Wacom is a constant weight 
which specifies a ratio of acceleration tracking to rate damping loop in the |i- 
synthesis. Sensitivity weightings for yaw rate and acceleration loop are used to 
specify the desired damping and acceleration tracking properties of the loop. The 
weight Wj for the damping loop is modelled as constant (gain) and the weight 
Wacc(s) is a transfer function 

k (7 v + l) 
W^,,(s)= "''^""^ '-, (16) 

PaccS + 1 

with parameters kacc, Zacc and pa^c are again derived from gain at low frequencies 
(steady state error for the acceleration loop), crossover frequency and high fre- 
quency gain. The weighting for the acceleration loop Wacc(s) does not have inte- 
gral action, since the linearised dynamics of the quasi-LPV system have integral 
action in the acceleration loop. Finally, constant weightings Wna and Wnr are in- 
troduced which cover the effects of measurement noise that corrupt the feedback 
channels. The actuator and the lateral dynamics are extended by parametric uncer- 
tainties, which are represented by the structured (real) block uncertainty 
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where Si, ,Si, ,...5j^ are real scalar perturbations -1 < ^ < 1 and correspond 

to the uncertainties in the model equations. The plant is brought to the general 
control configuration [14], [16] and the p-synthesis is conducted with DK- 
iteration algorithm using the Robust Control Toolbox [15]. After controller design 
and test of robust stability, step response tests were conducted with the linear de- 
sign model, containing model uncertainty. A test result for a higher super sonic 
Mach number is given in Fig. 3 as an example for the results achieved in linear 
simulations, where the upper part of the figure shows the damping loop and the 
lower part of the figure shows the acceleration tracking loop. 
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Fig. 3 Step response tests witti ttie design model including uncertainty at a higher 
super-sonic Mach number with damping (upper) and acceleration tracking (lower) part of 
the figure. 



4 Flight Controller Implementation 



Similar to the previous implementation of the robust controller [12] five lateral 
flight controllers were designed for the complete Mach envelope. Before imple- 
mentation, the controllers were reduced to 7*-order with additional check of ro- 
bust stability. In addition to the implementation of the robust controllers, tuned 
with the equilibrium linearised quasi-LPV system at a certain sideslip-angle, each 
controller consists of a state transformation for the specific Mach number, see 
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Fig. 4. Note that the centre-of-gravity acceleration can be used in the nonlinear 
model of yaw motion as a feedback value (Fig. 4). Sign changes have to be re- 
garded when implementing the LPV transformations for the pitch channel. The 
controllers were gain scheduled depending on the normalised air density as a func- 
tion of altitude 



^lal 



P(h) 

PQ 



(18) 



with p(h) , the air density corresponding to the current altitude and /7q , the sea 
level air density. Acceleration error and rate input to the controller were divided 
by the factor ^,^, . Note that the dynamic pressure for the state transformations has 

also to be adopted since the transformation is dependent on angle-of-attack or 
sideslip angle. The variation of thrust vector effects over altitude were not consid- 
ered in the scheduling approach and are assumed to be covered by the control ef- 
fectiveness uncertainties used in the controller design. During simulations over 
varying altitude a slight degradation of control performance towards slower re- 
sponse times could be observed, which is addressed to the TVC altitude dependent 
effects. 

Finally, signal filtering and processing were applied to the acceleration input of 
the controller. The filters were implemented in the nonlinear 6-degrees of freedom 
model only and are needed for two reasons. Firstly, the body bending effects that 
were not included in the controller design can reduce control performance in the 
acceleration path due to the high gain of the body bending Eigenfrequency peak. 
Therefore, a notch filter was implemented to filter the inertial measurement unit 
(IMU) measured acceleration. The notch filter frequency was scheduled depend- 
ing on motor burn phase. Secondly, the robust flight controller was designed with 
centre-of-gravity (COG) acceleration. To avoid lever arm effects in the accelera- 
tion signal due to IMU displacement from COG a lever arm correction filter was 
implemented as a second order filter with time variable lever arm l^ to 



C-Q^) „ 



. controller 



CAS 



f Nonlinear 
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Dynamics 



Body 
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Fig. 4 Flight controller implementation with sideslip-angle dependent state and input trans- 
formations T, and T,, respectively. 
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GacM=la^^ 2, (19) 

which is used to filter mainly low fi^equency effects of the lever arm rate bias on 
acceleration. Another reason for the band limitation is the body bending effects on 
the rate signal which should not be amplified. Input to (19) is the notch filtered 
yaw rate. The output of (19) is subtracted from the notch filtered measured accel- 
eration signal. 

5 Nonlinear Simulation 

Nonlinear simulations were conducted with two models. In a first step, a controller 
designed for a certain Mach number is implemented and tested in the nonlinear 
model of lateral yaw motion (1), which is extended by the CAS model. The con- 
troller response to acceleration reference steps is tested over the whole manoeuvre 
range at that Mach condition. In a second step, the controller is implemented in the 
nonlinear 6-degrees-of-freedom simulation as a three axis (roll, pitch and yaw) 
autopilot. The roll controller already in use for the ground-to-air missile was used 
to complete the three axis autopilot. The 6D0F model contains full nonlinear wind 
tunnel measured aerodynamics, a detailed model of the control actuation system, 
the body bending model, a motor model and a model of the thrust vector control 
system. Fig. 5 shows an acceleration reference step response series with the 
nonlinear model of yaw motion at a medium super-sonic Mach number. The ac- 
celeration is scaled on the maximum allowable acceleration over all flight condi- 
tions, which is determined by the structural limit. A slight tendency to overshoots 
can be seen towards higher acceleration reference values. This effect is due to the 
controller design at lower incidence angles with the fixed quasi-LPV model. How- 
ever, since the overshoot is less than 10%, a gain scheduling as a function of inci- 
dence angle is not required. Although additional performance may be gained with 
the introduction of a gain scheduling of inner- and outer-control loop. Note that 
there is no steady-state error in Fig. 5. This is because of the integral action of the 
quasi-LPV dynamics. Integral action and the resulting zero steady-state error are 
the result of an accurate equilibrium transformation. Steady-state errors can be ob- 
served if uncertainties are introduced to the model, for example to F(/?) or 

N{I3) . The lower part of Fig. 5 shows the resulting rudder position and the side- 
slip-angle, which are scaled on the maximum allowable rudder position and the 
global maximum sideslip-angle, respectively. 

An example of a simulation with the nonlinear 6D0F model is given in Fig. 6. 
The multivariable loop response is given for an acceleration step on the yaw chan- 
nel of 50% maximum possible acceleration value. Note that the missile motor is in 
burnout condition leading to a drop in missile velocity below Mach 1.0 and a re- 
sulting increase in the incidence angle towards the absolute incidence angle limit 
for the missile. After a relatively fast initial response to the step command, a small 
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Fig. 5 Response to acceleration steps with nonlinear yaw model at medium super-sonic Mach. 



steady-state error can be seen, which is suggested to derive from the drop of 
missile velocity. The multivariable loop coupling is small compared to the step in 
acceleration (see z-axis acceleration). This is also true for the nick/yaw rate and 
the roll rate, which is not shown in the Fig. 6. Similar results to Fig. 6 were 
obtained during step response tests with the nonlinear 6D0F model. A slight 
tendency to overshoots could be observed with reference steps leading to high 
incidence angles. Small steady-state errors were observed for rapidly changing 
velocity, that is on large manoeuvres without burning motor, or small manoeuvres 
with burning motor. 
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Fig. 6 Nonlinear 6DOF model response to an acceleration step command in the yaw chan- 
nel around Mach 1 during motor burnout. 



6 Results and Discussion 



A robust autopilot design using a transformation of the nonlinear yaw dynamics to 
a quasi-linear parameter varying system was presented. The robust autopilot was 
tuned with the quasi-LPV model at a low sideslip-angle for the yaw dynamics of a 
high-agile ground-to-air missile. The resulting autopilot was first tested in a 
nonlinear model of yaw motion and in a second phase tested in a detailed and vali- 
dated nonlinear 6-degrees-of-freedom model. The controller was designed for dif- 
ferent Mach numbers, used as operating conditions and scheduled over the air 



300 B.J.E. Misgeld, M. Darcis, and T. Kuhn 

density. Uncertainties of the quasi-LPV system dependent variable and control 
surface effectiveness were included to the design. Good results were achieved in 
simulations with a tendency to small overshoots (below 10%) and small steady- 
state error when leaving the operating conditions of a single controller. As a result, 
five controllers were designed to cover the full flight envelope of the missile. 
However, the controllers have to be combined using a blending methodology to 
obtain a reasonable flight controller, which will be the subject of future work. Fur- 
thermore, the quasi-LPV transformation functions contained parameters which 
were time dependent, like mass and inertia but were defined as constant parame- 
ters in the approach. It is suggested that the introduction of a time-dependency as a 
function of burn-time will lead to further performance gains of this approach. It 
should be noted, that the control system reaction to rudder/elevator saturation was 
not investigated in this study as there were no means of anti-windup measures 
included to this approach. In addition to that, the angle-of-attack and the sideslip- 
angle were assumed to be known, instead of using estimators for both values. The 
investigation of these effects will be subject to future work. 
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Part III 
Sensors, Data Fusion and Navigation 



A Single Frequency Strapdown Algorithm for 
Integrating IMUs in ECEF-Frame 



Johann Dambeck and Benjamin Braun 



Abstract. This contribution will introduce a strapdown navigation algorithm for 
integrating inertial measurement units (IMU), which - due to its inherent ability to 
propagate navigation states at the same frequency as the integrating IMU is pro- 
viding measurements - is called single frequency strapdown algorithm. The algo- 
rithm will be derived w.r.t. Earth-centred Earth-fixed reference frame. Numerical 
results for a simulated kinematic trajectory are presented. 

1 Introduction 

The integration of measurements of an inertial measurement unit (IMU) for 
propagation of (kinematic) navigation states like position, velocity and orientation 
is an integral part in many air, land and sea navigation applications. The derivation 
from Newtonian physics results in a system of first order ordinary differential 
equations for the navigation states given measurements of the specific force in the 
body-fixed frame (shorthand /j-frame) centred at and along/about the orthogonal 
(calibrated) IMU sensing axes w.r.t. inertial space. These inertial measurements 
are provided by so-called non-integrating IMUs. Since most precise IMUs are 
time-integrating, i.e. provide short equidistant time-integrals of specific force and 
angular rate, the commonly derived inertial navigation differential equations can- 
not be directly used. Common engineering practice is to calculate the required 
non-integrating IMU measurements by dividing the integrating IMU measure- 
ments by the integration time interval, but this corresponds to first order error 
terms and thus is only useful for low grade IMUs or applications with a high vi- 
bration level which would counteract the benefit of higher order terms in strap- 
down navigation algorithms. In contrast to this simple averaging approach, inertial 
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navigation experts have derived so-called dual frequency strapdown algorithms 
[9, 6, 8, 4, 1 1] based on Laning-Bortz orientation parameterization [7, 1] i.e. a rota- 
tion matrix is parameterized by the exponential of a skew matrix containing the 
three Laning-Bortz orientation parameters. These algorithms are "dual frequency" 
because integrating IMU measurements at a higher frequency are used to propagate 
navigation states at a lower frequency (relations of 3:1 and 4:1 have been pub- 
lished). This contribution will introduce a strapdown navigation algorithm for inte- 
grating IMUs, which - due to its inherent ability to propagate navigation states at 
the same frequency as the integrating IMU is providing measurements - is called 
single frequency strapdown algorithm. The algorithm will be derived w.r.t. Earth- 
centred Earth-fixed reference frame (shorthand e-frame). The paper is organized as 
follows: First, the navigation algorithm for non-integrating IMUs in e-frame is 
introduced in section 2. Then, the orientation solution using integrating gyroscope 
measurements is derived in section 3. In section 4 the transport rate is calculated 
as intermediate result for the position and velocity solution of section 5 using 
integrating accelerometer measurements. The resulting single frequency strapdown 
algorithm is presented as a template in section 6. Finally, numerical results for a 
simulated kinematic trajectory are shown in section 7. 



2 Navigation Algoritlim for Non-integrating IMUs in e-Frame 



Before addressing the single frequency strapdown algorithm for integrating IMUs 
the commonly known strapdown navigation differential equations expecting non- 
integrating IMU inputs are briefly revisited. These will serve as basis for the new 
algorithm in the sequel. Independent of the chosen navigation state parameteriza- 
tion the system of ordinary differential equations for propagating the navigation 
states using non-integrating IMU measurements is of the form 



z(0=f(z(0,s(r)). 



(1) 



where z(t) is the navigation state vector (containing position, velocity and orienta- 
tion) and s(/) is the sensor measurement vector (containing IMU measured specific 
linear force and angular rate). In the most general case the navigation state vector 
z(t) consists of the Cartesian position vector Xg(t), the Cartesian velocity vector 
Ve(?) and the rotation matrix Reb(t) where e represents the Earth-centred Earth- 
fixed reference frame and b the body-fixed frame. The sensor measurement vector 
s(?) is constituted of the vector of specific force in b-frame fi,{t) measured by the 
accelerometer triad and the vector of angular rate in i>-frame w.r.t. inertial (/-) 
frame (dibit) measured by the gyro triad: 



= (.):= 



'^A^)' 


v„(0 


R.(0 



and s[t): 



w* (0 
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Av, Av, Av, 

— I I I i > ^ 

f-3Af f-2Af f-Af t 

\ \ \ \ Single Frequency Algorithm Output (1:1) 

^^^^^^^^^^^^^^^^^^^^^^^. Dual Frequency Algorithm Output (3:1) 

Fig. 1 Input / Output ratio of single and dual frequency strapdown algorithms 

With this choice of navigation state parameters the nonlinear 1^' order ordinary 
navigation differential equations becomes [2]: 

x,(0 = v,(0 (2) 

v,(f) = R,„(0-f*W + y,(x,(f),f)-2«,,xv,(f) (3) 

R,„ (t) = R,, (0 £2,, (t) - £2„,R,„ (t) (4) 

where 

Xeit) (Cartesian) position in e-frame 

Ve(f) (Cartesian) velocity in e-frame 

Reh{t) Rotation from b- into e-frame, Rei,{t)Reb''(t) = I a det(Rf;,(f)) = +1 

(hit) Specific force in i>-frame 

0,7,(0 Angular rate of i>-frame w.r.t. /-frame, Q,i(0 := [co,fc(f) x] 

a),v Angular rate of e-frame w.r.t. /-frame, Q,e := [(B,> x] 

Y(,(Xe(f), /) Gravity vector incl. tidal acceleration in e-frame 

t Time 

This system of navigation differential equations in e-frame for a non-integrating 
IMU is nonlinear only due to the gravity field of the Earth (and tidal accelerations) 
and has time varying coefficients, i.e. the linear acceleration f,,(f) and angular rate 
(n»(0 measured by a non-integrating IMU. So far, a numerical integration routine 
of sufficient order like Runge-Kutta [5] could be used to numerically solve this 
system of first order differential equations since no closed-form solution is known. 
Integrating IMUs however measure time integrals of linear accelerations ti,{t) and 
angular rates C0,7,(0, i-c velocity increments Av;,(?) and angle increments A0,7,(/) 
defined by 

t t 

Ay,{t-At,t):= j f,(T)dr, AQ,,{t-At,t):= j o),,(T)dT. 

r-Al l-At 

It will be shown how to solve the system of navigation differential equations in e- 
frame (2), (3) and (4) for an integrating IMU with sufficiently high error order by 
keeping the integrating IMU input rate as navigation state output rate. Figure 1 il- 
lustrates the input/output ratios of the single and dual frequency algorithms. 
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3 Solution of the Rotational Initial Value Problem 

The orientation differential equation in e-frame (4) describes the propagation of 
the orientation of Z^-frame w.r.t. e-frame knowing the IMU measured rates i2,7,(f) 
and the Earth's rate of rotation f2,v,. Since the Earth's rate of rotation is very accu- 
rately known [10] it is independent of position and velocity and has no explicit 
time dependency. In this section the corresponding orientation difference equation 
incorporating integrating gyro measurements is derived. 

3.1 Decomposition and Discretization of the Rotational Initial 
Value Problem 

For simplicity the initial value problem (4) can be decomposed into two initial 
value problems 

R,, (0 = R„ (f )Q,,, R, {t, ) = R„,„ and (5) 

R,,(f) = R,,(On,,(0. R„(f„) = R„, . (6) 

The desired solution Reb{t) is obtained by multiplying the solutions of both initial 
value problems, i.e. 

R,„{t) =Rf^{t)-R,{t). 
The corresponding orientation difference equations can be written as 

R,,,(f + Af) = R,,,(f)-X(f,f + Af), R,,(f„) = R,,__ and (7) 

R,,{t + At) = R_,{t)-T,{t,t + At), R,,(r„) = R,,„ (8) 

where T^t, t + At) and T;,(/, t + At) are yet unknown "transition" matrices. If these 
recursive equations would be known the solution of our initial value problem (4) 
would be 

R^,,{t + /^) =Rl{t + At)-Ri,{t + 1^) = ^^ {t,t + /^)-Rl{t)-R,,{t)-%\t,t + /^) 
^ R,,{t + At) =Tj(/,/ + Af)-R^,(/)-T,(r,r + Ar). (9) 

3.2 Determination of the Transition Matrix of the e-Frame 

The solution of the initial value problem (5) with constant coefficients due to a 
constant Earth's rate of rotation is 



K{t) = K-^ 



n„.'('-'o) 
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By definition R,>(/) changes by a simple rotation about the Earth's axis of rotation, 
i.e. z-axis of e-frame, according to the Earth's rate of rotation (co,,. := {0,0, COte)^) 

cos{a)i^,At) -sin(ft^^^,Ar) 



sm{a>,^,At) cos{a>;^,At) 
1 



(10) 



3.5 Determination of the Transition Matrix of the b-Frame 



Between two arbitrary coordinate frames R(f ) = R(f ) ■ il{t) and R(f, t + At) = 

R(t)T(f, t + At). The general transition matrix T(f, t + At) is derived by expanding 
the general rotation matrix R(f, t + At) in a Taylor series 

(11) 



Af Af 

R{t + At) = R{t) + R{t)-At + R{t)-=— + R{t)- — + 0{At') 



and subsequently expressing the time derivatives of the rotation matrix R(/) by 
the orientation differential equation R(f) = R(f)i2(f) and the corresponding time 
derivatives 

R = R n, 

R = R a+R Q = R [n^+n], 

R = R-[n'+n]+R-[nn+nn+n] = R-[n'+2iifi+fi£i+ii]. 

Substitution of these time derivatives of R into (11) gives 



R{t+At) = R{t) 



At^ At^ 

i^+nAt+[n^+n) — +[n^ + 2nn+nn+n) — +o[At'^) 



A0 



ib 



Ao; 



4-^ 



t-At 



t + At 



Fig. 2 Subsequent integrating IMU measurements. 



310 J. Dambeck and B. Braun 

One can easily identify the term in square brackets as transition matrix T(f, / + At). 
It just depends on the skew rate matrix Q, and its time derivatives 

T{t,t + At)^I,+n-At + [n^+£l) + (fi'+2Qli + Qn + £2) + 0{At*). 

(12) 

For the particular recursive form (8) of the initial value problem (6) the skew rate 
matrix Sin, and its time derivatives are required to calculate the transition matrix 
Tb{t, t + At) which have to be extracted from integrating IMU measurements. For 
this purpose, two subsequent integrating gyro triad measurements A©;,,"^ and A©;,," 
(cf. figure 2) are expanded in Taylor series 

'+Ar ,2 A .3 

A0;:= j n,,(T)dT = n,(r).Ar + n,(r). — + n,(/). — + 0(Ar^), 

.; .; (13) 

A0:,:= j n,,(T)dT = n„(/)-A/-n,,(f)-^+n,,(/)-^+o(Af^). 

These expansions represent relations between subsequent integrating gyro meas- 
urements A0,7, and the non-integrating gyro measurement £2,7,(0 and its time de- 
rivatives at time t. Inverting these Taylor series expansions and substituting the re- 
sult into the expression for the transition matrix T/,(;, t + At) would give the 
desired result. Alternatively, we can express the transition matrix T;,(f, / + At) by 
products of two subsequent integrating gyro measurements and identify the coeffi- 
cients by direct comparison 

%{t,t + At)=\,+ «, A0; + fl, A©:, + a,AQl' + a.AQlAQ:, + flj A©:, A©; 
+af^A&:i^^ +a-,A&*^f^ +h.o.t. 

With the angular increments (13) the required double products in (14) are as in 
(13) up to 3"* order terms in the time increment At 

A©;-A©;=n^o-Af'+K(0^,.W+^,.Wi^,.W)4+^K) 

A&l-A&l=Q.l{t)-At'-[n,,{t)^,,{t)-n,,{t)n,,{t))-^ + 0[At') 
A&l-A&l=Sll{t)-At'+{n,,{t)^,,{t)-n,,{t)n,,{t)\^ + 0[At') 

A©:,-A©:,=nf,(0-Af^-(n,,(f)n,,(f)+n,,(f)n,(0)-^+oK) 

and the triple product of the most actual integrating gyro measurement is corre- 
spondingly 

A©; -A©; -A©; =ni {t)-At'+o{At'). (i6) 

(13), (15) and (16) are substituted into (14) and the resulting equation is termwise 
compared with (12) to determine the coefficients of (14). 
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Comparison of coefficients linear in Q.ih{t) and its time derivatives gives 
three linear equations (of which two are linearly dependent) for two unknown co- 
efficients 
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Comparison of coefficients bilinear/quadratic in Q,7,(r) and its time derivatives 
gives three linear equations for four unknown coefficients (underdetermined, thus 
we use the degree of freedom to preserve symmetry and preferably eliminate 
terms using oldest measurements by setting a^ := 0) 
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Comparison of coefficients cubic in Q.ib(t) simply gives il,^ (f) • Af^ 

Substituting the derived coefficients ai 
matrix T,,(f, t + At) results in 



aj in formula (14) for the transition 



T„{t,t+At) = i, +A0; +iA0;^ -^(a0;a0:, -a0„a0;)+ia0;/ +c»(Af^) 

(17) 

With the determined transition matrices (10) and (17) the recursive propagation 
equation for the rotation matrix describing the orientation of the i>-frame relative 
to the e-frame in 4* error order is completely determined. Herein, beside constant 
coefficients, just two successive integrating gyro measurements are required. Us- 
ing a longer history of integrating gyro measurements would mathematically give 
a more accurate result, but due to sensor noise and platform vibrations one can in 
general not expect higher orientation accuracy when incorporating higher order 
terms. To conclude, it is recommended to orthonormalize R^hit + At) from time to 
time in order to reduce numerical inaccuracies [3]: 
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R*W = R*(0[RJ.(OR..(Or'- (18) 

4 Angular Rate between e- and b-Frame from Integrating 
Gyro Measurements 

The rate matrix U,eb{t) will be required for the solution of the translational initial 
value problem. Hence a formula for the computation from integrating gyro meas- 
urements will be derived in this section. For the calculation of Q,eh{t) from Qft(/) 
the Earth's rate of rotation has to be transformed into i>-frame and then compen- 
sated, i.e. 

Q,7,(f) is calculated from gyro measurements by again making use of the Taylor 
expansions (13). The most trivial inversion of the Taylor series gives the central 
difference quotient 

and therefore the rate matrix Q.eb(t) finally gets 

n^.,{t) = ^{A0l+A0l)-Rl{t)n,^,R^„{t) + O{At'). (19) 

Later, when collecting all partial results in the single frequency strapdown algo- 
rithm template in section 6, it will become clear that the 2"** error order of the cen- 
tral difference quotient is sufficient to use. 

5 Solution of the Translational Initial Value Problem 

The translational differential equations in e-frame (2) and (3) describe the propa- 
gation of position and velocity w.r.t. e-frame given the IMU measured specific 
forces th{t), the local gravity vector Yc(Xe(?)), the Earth's rate of rotation CO,,, and the 
solution of the rotational initial value problem Reb(t)- In this section difference 
equations for position and velocity incorporating integrating accelerometer meas- 
urements are derived. 



5.7 Non-integrating from Integrating Accelerometer 
Measurements 

Just like the angular rate Qibit) in the orientation differential equation had to be re- 
placed by integrating gyroscope triad increments A0,7,(/), the specific force fi,(t) in 
the velocity differential equation has to be expressed by integrating accelerometer 
triad increments A\i,(t). Thus, A\i,(t) is analogously to (13) expanded in a Taylor 
series 
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Af^ 



Av:- j f,{T)dT^f,{t)-At + i{t)- — + 0{At') 



2! 



At' 



Av-:= j (,{T)dT = {,{t)-At-i{t)- — + 0{At') 

t-At ^• 

and due to the lower error order required again solved by trivial series inversion 

Avt+Av: 



Av;+Av,;=2f,(/)-Af + C)(A/') ft{t)- 



Ayl-Ayl=i{t)-Af-+0{At') ,i,^_-t -„ 



2At 
Avt-Av: 



At' 



-0[At') 
-0[At'). 



(20) 



5.2 Velocity from Integrating Accelerometer Measurements 

The velocity difference equation is derived by Taylor series expansion 

y^{t + At) = y^ (f ) + v^, {t)-At + v,, {t) + (9 ( Af ^ ) . 

v^ (?) is given by the velocity differential equation (3) 

V. {t) = R., {t)h {t)+l. (x, (f))-2co„ X V, (f) 
and its time derivative by 

Thus, using (20) and R^,j, (t) - R^^ (O^e* (0 ' '■^^ velocity time derivatives are 

Avt+Av: 



(21) 



v.(0=R..W- 



v.(0=R..W 



2Af 



-y,(x„(f))-2(o„xv„(/) + 0(Af^) 



li,,(/)-^^'^+^^*-^^'^-^^'^ 



2A? 
2(i),^x\^,{t) + 0[At'). 



At' 



+ l^T ^ ^(') (22) 



The Cartesian gravity vector y^ixXt)) and the gravity vector gradient 3ye(Xj(/))/3x^ 
can be approximated by the gravity field of an equipotential ellipsoid of revolution 
[2]. For tactical grade IMUs this model is sufficient which furthermore can be 
truncated after the J2 parameter as defined in [10]. For navigation and strategic 
grade IMUs more accurate models are required. 
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5.3 Position from Integrating Accelerometer Measurements 

Analogously to (21), the position difference equation is obtained by expansion 

x^,{t + At) = x^,{t)+ x,{t) ■At+ x^,{t) ■=—+ ■x{t) ■=— + 0{At'). (23) 



yAD 



v.W 



v„(0 



The time derivatives of Xeit) can be substituted by the known velocity Ve(f) and the 
velocity time derivatives (22). 



6 Single Frequency Strapdown Algorithm in e-Frame 

In this section, equations (9), (10), (17), (18), (19), (21), (22) and (23) required for 
propagation of position, velocity and orientation are combined in a template, con- 
stituting the single frequency strapdown algorithm. 



Input: 

• Integrating IMU measurements: (Av^ , A©^^ ), (Av^ , A0^ ) 

• Previous navigation states: x^, it), v,, (f), R,,^ it) 

Single Frequency Algorithm: 

• Auxiliary equations 



R.(0 = R.(0[Rl(OR.(Or' 

il^,,{t)=^{A&l+AOl)-Rlit)n,^.RAt) +o{^t') 

T, (0 =i3 +A0; +iA0;^ -^(a0;a0- -a0-a0;)+ia0;^ +o{At') 

+0{At') 



v,(0 =R,(r)^<±^ + y„(x^,(r))-2a,,,,xv,(r) 



v.W =R,.W 



2Af 



2At 



At' 



State propagation 

At^ At^ 

\it + At) =x^,(f) + v,(f)Af + v,(0 — + v„(f) — 
Af^ 
\^{t + At) =y^(t) + \^,{t)At + \^{t) 

R,{t + At)=R]{co,At)Rjt)T,{t) 



+0{At') 

+0{At') 
+0{At') 



Output: 

• Current navigation states: 



x^, (f + Af), v_,(f + Af),R,,j(f + Af) 
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Gravity and gravity gradient in e-frame could be modelled by [2] 



Ye.i = GM 



f f 2\\ 

J- a J A hr + 3 — ^0,-3 y 

b^ I 2. n^ X ^ X 



+ al{\-d.^)x^, ; = 1,2,3 



dx. 



-CM 



1 X X 



\ 



3 ,1 ^e^e 

--GM a-JA ^^,-5^^ 

"II IP -' II ir 

^ X X 



+ — GM a^J, 
2 ' 






-iGM a-jA-^S,,5p-5^^8,, \ + GM a)-^[5,j-5,,d^,), i J = 12,3 

[h\\ Kll ) 

with GM and J2 specified in [10] and ^,= 1 for i=j and <^,=0 for i^j. 



7 Numerical Results 

The performance of the single frequency strapdown algorithm was analyzed by 
means of numerical simulation. Especially the accuracy and the robustness against 
noise were considered. For the sake of analytically knowing the derivatives differ- 
ent sinusoidal functions in the components of the position vector were chosen as 
kinematic trajectory (cf. figure 3). 100 Hz error-free integrating IMU measure- 
ments were easily derived from the analytical trajectory. By numerically integrat- 
ing these generic measurements over two consecutive sample time intervals 
error-free velocity and angle increments Av;, and A9,fc were generated. For the 
analysis of the accuracy, the error-free IMU measurements were processed. For 
the verification of the robustness against noise, white noise was added to the 
generated IMU data. The results are additionally compared to the often used 
averaging approach which converts the integrating IMU measurements to non- 
integrating ones before integrating them with the conventional strapdown equa- 
tions and Runge-Kutta integration scheme. The non-integrating measurements are 
obtained by dividing the velocity and angle increments by the sample time, ft,{t) = 
A\h{t-At,t) I A? and Giib{t) = A6»(f-Af,f) / At, which correspond to a T' order Taylor 
series expansion of Avj(f-Af,f) = \',_A,{bi'^ dr and A9,4(f-Af,f) = \',_^, co,;/^ dr. i'hit) 
and 00,7,(0 thus can be interpreted as mean specific forces and angular rates over 
the last time step A?, which explains the naming averaging approach. Figure 4 
shows the position and orientation errors with error-free IMU measurements. The 
horizontal and vertical position errors remain below 1 .5e-3m. The orientation error 
does not exceed 6e-ll°. Figure 5 illustrates the performance of the single fre- 
quency strapdown algorithm with measurement noise (Oav = rn/s, Oas = 2e-8 rad). 
The position error is lower than 3m. The orientation error remains below 2e-5° in 
roll, pitch and yaw axis. 
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Fig. 3 True trajectory 
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Fig. 4 True position, velocity and orientation along trajectory 
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Fig. 5 Position and orientation error with Single Frequency Strapdown Algorithm and er- 
ror-free IMU measurements 
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Fig. 6 Position and orientation error with Averaging Approach and error-free IMU meas- 
urements 
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Fig. 7 Comparison of position, velocity and orientation error with noisy IMU measure- 
ments (o^^=le-4m/s, OAe= le-6°) ■■■■ Averaging Approach, ^-^ Single Frequency Strapdown 
Algorithm 

8 Conclusion 

The computational effort of the dual frequency algorithm is extremely low, but it 
has two drawbacks. First it is based on Laning-Bortz orientation parameters which 
lead to a quite nonlinear orientation differential equation in Laning-Bortz parame- 
ters, which is commonly approximated before it is solved. Second dual frequency 
strapdown algorithms require a higher integrating IMU input rate to provide the 
lower navigation state output rate. 

The single frequency strapdown algorithm presented requires some more com- 
putations, but gives a result with a very transparent error order and allows to output 
the propagated navigation states with the same rate an integrating IMU is providing 
its measurements. This approach can easily be adapted to other parameterizations 
of the navigation state vector and the usage of quaternions as orientation parame- 
ters. The provided simulation results verify the accuracy of the single frequency 
strapdown algorithm and show its robustness for a suitable trajectory. 
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Broadband Wind Estimation Algorithm for 
Gust Load Alleviation 

Arndt Hoffmann, Kai Loftfield, and Robert Luckner 



Abstract. Wind disturbances especially vertical gusts and turbulence can deterio- 
rate the quality of measurements performed by the payload of utility aircraft. Gust 
load alleviation systems can reduce this effect significantly if the atmospheric dis- 
turbances are determined precisely and broadband up to the frequency range of the 
short period mode. A novel approach to determine vertical gusts and turbulence that 
can be used for future implementation in a gust load alleviation system based on 
feed forward control is presented. This approach uses a linear aircraft model of the 
longitudinal aircraft motion combined with the Dryden Turbulence Model within a 
Kalman filter framework. For performance analysis it is compared to an algorithm 
using simplified flight mechanical relations. For proof of the concept and its robust- 
ness, different kinds of disturbances such as discrete gusts and a bias in the angle of 
attack measurement are simulated and discussed. 



1 Introduction 

For a variety of missions, such as airborne gravity measurement, the flight of a util- 
ity aircraft has to be smooth even in disturbed atmosphere. In such a mission the 
aircraft is operated at low altitudes for optimal measurement quality. As a conse- 
quence the aircraft is often affected by gusts and turbulence producing additional 
aerodynamic forces and moments. The resulting aircraft accelerations disturb mea- 
surement quality and therefore reduce mission effectiveness and affect the pilot's 
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and the mission-engineer's workload. Gust load alleviation systems based on feed 
forward control can alleviate accelerations significantly if the atmospheric distur- 
bances are determined precisely and broadband up to the frequency range of the 
short period mode. The predominating influences on the utility aircraft Stemme S 1 5 , 
a powered glider with a high aspect ratio and a single propeller engine, are vertical 
gusts and turbulence and therefore the wind-induced angle of attack (AOA). They 
cause additional vertical accelerations. For a broadband computation of the not di- 
rectly measurable, but observable, wind-induced AOA, flight mechanical variables 
have to be measured accurate and broadband. 

In Wise [1] an Extended Kalman filter (EKF) is used to determine the AOA and 
the angle of side slip (AOSS) based on the nonlinear six degree-of-freedom equa- 
tions of motion and a detailed aircraft aerodynamic model. The EKF framework 
uses inertial measurements of Euler angles, body rates, body acceleration and mea- 
surement of dynamic pressure. This approach was successfully flight tested in the 
X-45 program. Heller |l2| and Myschik O use a complementary filter approach 
based on linear equations to determine AOA and AOSS to extend the bandwidth of 
these signals. Braga |4|| shows a low computational cost method dealing with time 
varying noise statistical properties using a new approach for an adaptive EKF. The 
method is validated in flight path reconstruction application, with simultaneous air 
data calibration for AOA, AOSS and static pressure sensors. Combining a linear 
model of the short period motion and the Dryden Turbulence Model, Hoffmann ||5l 
has shown a Kalman-Bucy filter approach for precise and broadband wind-induced 
AOA determination in presence of atmospheric turbulence and observation noise. 
Myschik ||6l discussed an integrated wind estimation/navigation system for use on 
general aviation aircraft. This integrated system enables on board determination of 
the actual wind condition and aerodynamic flow angles using inertial navigation 
data. A method for estimating wind fields for small and mini unmanned aerial ve- 
hicles is described by Langelaan Q. The approach utilizes sensors that are already 
part of standard autopilot sensor suite. An approach how wind AOA can be com- 
puted using relations of flight mechanic variables that can be measured in sufficient 
quality is shown by Hahn fSl. 

In this paper a method using a linear aircraft model of the longitudinal aircraft 
motion combined with the Dryden Turbulence Model within a discrete Kalman fil- 
ter framework is described. For proof of the concept and its robustness, different 
kinds of disturbances such as discrete gusts and a bias in the AOA measurement are 
simulated and discussed. The results of this approach are compared with results of 
the approach described in lISJ. 



2 System Model 

In this section the linear aircraft model and the Dryden Turbulence Model are de- 
scribed and it is shown how they are combined, for analysis purpose and Kalman 
filter design. 
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2.1 Notation 

In the aircraft longitudinal motion that is considered here the flight path velocity Vj^ 
and the airspeed V^ are two dimensional vectors in the aircraft's plane of symmetry. 
The velocity components in x-direction are referred to with the letter u, the ones 
in z-direction with the letter w. The second index defines the reference frame (g = 
geodetic, b = body fixed). The velocity vectors are 



YAb^ 



UA 
Wa 



COS a 
sin a 



Va, Zk,= 



UK 
WK 



cosy 
— sin 7 



Vk 



WAb ,, 

tana = , Va 

UAb 



9 9 VV/fa 



4 



^K^ 



where a is the angle of attack (AOA) and y is the flight path angle. 



2.2 Linear State Space Aircraft Model 

The nonlinear equations of motion for an aircraft can be written in state- space form 
as follows: 



y^,{t)=c{x^,{t),u^,{t),z{t),t) , 



(1) 
(2) 



where x^,^, is the state vector, m^,^. is the control input vector, y is the output vector 
and z is the disturbance vector. The functions a{), b() and cQ model the nonlinear 
state dynamics and measurements. This 6 degree of freedom model is linearized for 
horizontal flight, with respect to the state vector x^^, the control input vector u^,^., 
the output vector y and the disturbance vector z, resulting in the following linear 
state-space representation, 



y . = c 
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(3) 
(4) 



with process noise (^ used to model system and observation noise C, to model 
measurement uncertainties. The vector elements for the longitudinal motion are: 
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The aircraft states are body axis pitch rate qK, kinematic AOA a/f, flight path ve- 
locity Vk, pitch angle and altitude H. The output vector consists of the body axis 
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pitch rate qK, the AOA a, the airspeed Va, the flight path velocity Vk as well as 
the pitch angle 0, the altitude H and the vertical speed H. Input is the elevator de- 
flection T] as well as the thrust power setting r\F- Wind-induced AOA aw and the 
horizontal wind velocity u-w are the disturbances. Process noise t, is assumed to 
be zero mean Gaussian white noise with covariance Q . Observation noise C, is 
assumed to be zero mean Gaussian white noise with covariance R . 



R = diag^ 



rad/s 



0.166x;r 
180 
0.25 x;r „„ j 

^^Q-rad 
0.009 m/s 
0.009 m/s 

0.25 x;r „„ j 

^^Q-rad 
0.0001 m 
0.027 m/s 



Q =diag^ 



0.166x;r „„ j /„2 
180 ^^^1^ 

^gQ- raa/s 
0.009 m/s2 
^gQ- rad/s 
0.0001 m/s 



(5) 



The model is derived for horizontal flight for a reference state of Va = 50 m/s, 
H = 100 m, no wind and a reference flight path angle 7=0°. 

2.3 Disturbance Model 

The Dryden Turbulence Model is an approximation of real gust Power Spectral 
Density (PSD). To generate the PSD for simulation purpose, zero mean Gaussian 
white noise r_{t) with covariance I is shaped through a Dryden filter as shown in Fig. 
[U For vertical and horizontal turbulence the transfer functions are given by Q and 
Q, where a is the standard deviation, L is the characteristic wave length, (O is the 
characteristic cut off frequency and T the characteristic time constant. The variable 
ww specifies the vertical and u-w the horizontal wind component. 



white noise 
source 



Dryden 
Filter 



w.(t), ii„(t) 



Fig. 1 Turbulence simulation 



rww \^) 



o^.Z, 



1 



?\/37;, 



Vk Oh, 



Fuw (^) = v 2cT2r„ 



(l+sZ,) 
{l+sT,)' " Vk cOu 



(6) 
(7) 



The filter characteristics depend on height, terrain roughness and wind speed. The 
characteristic values of the Dryden filter for the reference state of Vk = 50 m/s, 
H = 100 m and moderate turbulence are, according to 191, as follows: 



L„ = 100 m, L„ — 260 m, a„, — 1 m/s, Ou = 1.38 m/s . 
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Fig. 2 Velocity vector diagram in the aircraft's plane of symmetry 

The kinematical relation of aircraft and wind states for the longitudinal motion is 
given by the velocity vector equation V_jf = V^ +Vjy and is shown in Fig. [21 The 
two components of the wind velocity vector Vjy„ in the geodetic reference frame 
are uwg and wwg- Using the relation shown in Fig. [21 the wind-induced AOA aw 
can be calculated with ([SILj. 



(8) 



(9) 



wwg uwg 
sinaiv = — ^cosyH -s,\ny . 

For horizontal flight (y ~ 0°) and small angles aw is approximately 

WWg 



aw 
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Using the linear relation ^ and the transfer functions Q and Q, the Dryden Tur- 
bulence Model can be written in observer state space form as: 



tory ~ Aory -Dry ' 
2 = Qory -Dry 
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And in detail: 
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(12) 



The state a^ is an internal state without physical interpretation. Model uncertainties 
in the Dryden filter are regarded as process noise ^ory It is assumed to be zero mean 
Gaussian white noise with covariance Q 

=Drx 



Sign convention as in 1101 . 
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io„ 



: diag^ 



5 X 10-5 X ^ rad/s2 

5 X 10-4 X ^ rad/s 

3.5 X 10-^ m/s2 



(13) 



2.4 System Model 

The aircraft and the disturbance model have to be coupled to become the system 
model that is used for filter design. The linear aircraft model (O and (|4| can be 
combined with the Dry den Turbulence Model Q and Q. The system model state 
space equations can be written explicitly as follows: 



(14) 

(15) 
Since the input vector to the Dryden Turbulence model r_ is not measureable, the 
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[r] is regarded as a part of the process noise. In short the system 



model state space can be written as: 



X = Ax+Bu+^ 
y^Cx+1 



(16) 
(17) 



with: 
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(18) 



(19) 



where Q is the covariance matrix used for filter design. 



2.5 Frequency Range Analysis 

To identify the frequency range where significant normal accelerations occur and to 
define the frequency range, for which the disturbance signal has to be reconstructed 
for feed forward gust alleviation, the amplitude response of the aircraft |F„. aw I > th^ 
Dryden filter l/^zwrj and the combination of both |^;;-ri| is plotted in Fig. [3] The 
characteristic high-pass behaviour of the aircraft can be clearly seen as well as the 
low-pass behavior of the Dryden filter. In combination two maxima result, one close 
two the Eigen-frequency of the phygoid mode (0)p/, =0.18 rad/s) and the second 
close to the short period mode {(Osp — 4.7 rad/s). For gust load alleviation both 
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Fig. 3 Amplitude response 

maxima are of interest and therefore the disturbance signal has to be reconstructed 
up to a frequency range of approximately 10 rad/s. For this analysis aeroelastic and 
instationary aerodynamic effects are not taken into account. 

3 Kalman Filter 



The presented approach uses a Kalman filter to estimate the states of the linear 
system model ( fT6b and (fTTl l. 

The Kalman filter estimates the state vector x by processing the output vector y. 
This estimation is generated in two phases. The predict algorithm calculates a value 
according to the state equation and estimates the uncertainty of this value. Then 
the update algorithm computes a weighted average of the predicted value and the 
output value. To receive an optimum estimate, a Kalman gain K^ is computed for 
which the trace of the estimate error covariance matrix P_ is minimal. The estimate 
error covariance matrix is defined as £ = E [(x — f ) (x — i)^] , where E denotes the 
expectation operator and x the estimated state vector. 

The notation used to illustrate the two different phases has two subscripts. The 
first subscript indicates the point in time, while the second indicates the process- 
ing of the A:-th set of measurements. The predict algorithm propagates X]^_i\f^_i to 
Xk\k-\ ^nd the update algorithm propagates xm_\ to xm. Where only one value is 
calculated for a time step, the second subscript is dropped. 

Since it is fundamental to software processing and the nature of the Kalman filter, 
a discrete system model has to be used. Therefore the continuous linear system 
model ( IT6l ) and (fTTl l is discretized. This is done using a zero-order hold and results 
in the following equations. 






(20) 
(21) 
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Fig. 4 Block diagram of plant and Kalman filter 

The equations for the predict and update steps for the state vector and the estimate 
error covariance matrix are: 



Predict: 



U\k-1 — £a-l|A:-l +Qlik 



F P 



F 



T 



h=k\k-\ =hzk-\\k-\= ' = 



(22) 
(23) 



Update: 



K, = g,^,_,K\L+Kt,^,_,K^ 



h\k = -h\k-i +Ek \y.k~=-^\i< 



t,^,= [l-K,K)E,^,_, 



(24) 
(25) 
(26) 



with S and T_ being the discrete process and observation noise derived from the pro- 
cess and observation noise described in Sect. 12.41 In addition, the Kalman filter can 
estimate constant errors (bias) in the measurement . The bias bj^^ can be interpreted 
as an additional input vector in the output equation ( |2T1 | with the new feedthrough 
matrix D. Since ^ is a constant vector a new equation is added to the system model. 



a+i =Lxj, + Gui,+^ 



(27) 
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^•+1 = h 



(28) 
(29) 



Equations dZTJ ) - ( l29l l can be written in state space representation of the following 
form: 
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[h] = [m] 




+ [^] • 



[k 



(30) 
(31) 



Equations (l30t and (l3T1 l are of a form on which the Kalman filter is applicable. The 
block diagram in Fig. |4] shows the structure of the system model and the Kalman 
filter. For simplification, the propagation of the estimate error covariance is not ex- 
plicitly shown in the block diagram. 



4 Algorithm Using Simplified Flight Mechanic Relations 

For the longitudinal motion the following equation is exact as shown in Fig.|2] 

Ow =Q-y-a . (32) 

Due to measurement quality the flight path angle is approximated by 7 « arcsin ( y- 
With this approximation the estimation equation for aw reads 



awDLR » - arcsin ( tt I ^ « 



(33) 



This estimation was successfully implemented in a gust load alleviation system by 
the German Aerospace Center (DLR) |i8J and is here used for comparison to the 
presented algorithm. 



5 Simulation Results 

The system model and the Kalman filter, as shown in Fig.|4l were implemented in 
Simulink. The simulation sample time was 0.016 s. Fig. [5]- Fig.[7]show simulation 
results for the derived system model, the Kalman filter and the simplified flight 
mechanical approach. The aircraft is in trimmed condition, disturbed by Dryden 
turbulence as well as discrete 1 — cos gusts for aw (f = 10 s and r = 70 s) and uw 
(t = 50 s) and responding to elevator deflection. Simulated states of the real system 
are labeled with the index "real", estimated states with "est" and the simplified flight 
mechanical approach with "DLR". The strength of observation and process noise is 
as defined in Sects. [231 and l2. 31 The simulations were performed on a standard PC. 
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Fig. 5 Simulated time histories of the turbulent states and their estimates 
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Fig. 6 Error analysis of the turbulent states and their estimates 



The not directly measurable turbulence states and the bias state (ahias = 1°; t = 50 
s) are compared with their estimates in Figs.[5]and|6l 

Fig. |6] shows time histories of the error (A — x^eai — Xest) between the state vari- 
ables and their estimates and relative frequency distributions of the error. The dot- 
ted lines indicate the mean value and the dashed lines the standard deviations of 
the error. The standard deviation, calculated from the trace of the estimation error 
covariance matrix Gcov is shown for the estimations computed by the Kalman filter 
and is a measure for the quality of estimation. The correlation between the turbu- 
lence states, the bias and the estimated states computed by the Kalman filter show 
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Fig. 7 Fourier Analysis of aw (left) and uw (right) and their estimation 

to be sufficient for gust load alleviation. The simplified flight mechanical approach 
estimates the wind induced AOA well up to the occurrence of the bias in the AOA 
measurement. Due to this bias the relative frequency distribution for the estimated 
wind induced AOA error is broad and flat. 

Fourier analysis in Fig. [T] shows a broadband correlation of the estimated turbu- 
lence states and the simulated states which emphasises the correlation seen in the 
time histories. The frequency range requirement derived for gust load alleviation, 
i.e. good agreement between real and estimated turbulence up to 10 rad/s, is easily 
met. 



6 Conclusion 

The application of a discrete Kalman filter using a combination of a linear aircraft 
model and the Dryden Turbulence Model to estimate the not directly measurable 
wind states has proven to be feasible and shows better results than an algorithm 
using simplified flight mechanical relations. Simulation results show a good and 
broadband correlation between the states and their estimates up to the short period 
mode which is sufficient for gust load alleviation. The estimations are robust against 
variations of the disturbance model and a bias in the AOA measurement. 

For operational use in an aircraft this approach shall be developed further using 
an extended Kalman filter (EKF) based on the described concept and a non-linear 
aircraft model. The EKF has to be carefully tuned by observation and minimizing 
of the estimation error covariance matrix. The observability of each state under dif- 
ferent conditions, e.g. no turbulence, has to be analyzed. 
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Interval Analysis as a System Identification Tool 

E. van Kampen, Q.P. Chu, and J. A. Mulder 



Abstract. This paper shows how the theory of interval analysis can be utilized 
for system identification by applying it to two aerospace related problems. Interval 
numbers form an extension of the regular crisp numbers and they were first intro- 
duced in the 1960's to investigate how rounding errors propagate through systems 
implemented on digital computers. Since then interval analysis is mainly used as a 
tool for solving global nonlinear optimization problems where it is one of the few 
methods that can guarantee to find the global minimum of any nonlinear cost func- 
tion. Here the properties of intervals are employed for system identification tasks. 
The first application is optimization of human perception modeling, i.e. identifying 
how a human pilot perceives visual cues and motion cues, based on the pilot in- 
put and output. In the second example trim points of a nonlinear aircraft model are 
identified simultaneously, with the guarantee that all trim points are found. 

1 Introduction 

System identification is a crucial part in many engineering problems. An example 
is in adaptive flight control, where an online aerodynamic model is required by the 
controller to adapt its control strategy after a structural failure or a control surface 
failure. System identification consists of defining a model structure (Fig. [Hi and then 
optimizing the parameters in this model such that the model approximates the true 
system as closely as possible: 

3' = /(x) .v = /(x;p) min/(p) = |b-:y(p)|| (1) 

peP 

with X the input vector, p the parameter vector, y the true system output, and y the 
model output. 
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Fig. 1 First step of system identification; selecting the model structure. 

This equation shows that system identification is a minimization problem that, 
depending on the model structure, can be nonlinear and have multiple local minima. 
A large number of methods is available to solve these types of problems, from which 
gradient-based methods and genetic algorithms are among the most frequently used. 

With gradient based methods are meant any algorithm that directly or indirectly 
uses the slope of the cost function to find a (local) minimum. Some algorithms use 
this slope or gradient directly, such as the Newton method, while others still use 
slope information in a more subtle way. The Nelder-Mead algorithml 1 1 for example 
compares the function evaluations of the vertices of a given simplex and acts based 
on the ordering of these function evaluations, which can be seen as an indirect way 
of using slope information without explicit evaluation of the cost function derivative. 

Genetic Algorithm! 2 1 optimization methods are based on principles of biological 
evolution, such as natural selection and genetics. Both gradient-based optimization 
methods and genetic algorithms can get stuck in a local minimum of the cost func- 
tion and their solution depends on the initialization of the algorithm. With a different 
starting point or population, a different solution can be found, and it is not possible 
to determine when the global minimum of the cost function has been reached. 

In this paper interval methods are applied to the optimization problem. Interval 
methods are guaranteed to find the global minimum of the cost function, resulting 
in the optimal set of parameters. 



/./ Interval Analysis 

Interval numbers are an extension of the regular numbers and consist of a lower 
bound, an upper bound, and all the real numbers in-between these bounds. 

[x] = [fl,Z7] = {x* eRlasCjc*^;^?} (2) 

inf([x])=a (3) 

sup([x])=Z7 (4) 

Interval analysis was introduced in the 1960's by Ramon Moored, who used it 
to analyze how rounding errors propagate on limited precision digital computers. 
Later, interval analysis was found to be an excellent tool for solving global nonlinear 
optimization problems H). 
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The basic arithmetic operations on interval numbers are defined as: 

HOb] = {x*0/|x*e[x],/e [y]} 



(5) 



[a,b] + [c, d] — [a + c^b + d] 
[a,b] — [c,d] = [a — d,b — c] 
[a^b] ■ [c,d] = [min (ac, ad, be, bd) , max [ac, ad, be, bd)] 



[c,d] 



[a,b]-[l/d,l/c] ifO(^[e,d] 



(6) 
(7) 
(8) 

(9) 



All interval arithmetic operations are based on the inclusion principle, sometimes 
referred to as the fundamental theorem of interval analysis, which states that the 
outcome of the operation on a subset of the input interval arguments is included in 
the outcome of the operation performed on the complete input intervals. The subsets 
can be smaller intervals or crisp numbers (thin intervals): 



xi e [xi] ,x*2 e [x2] , ...,x* G [x„] ^ 

f{xl,xl...,x:)c[f{[x,],[x2],...,[x„])] 



(10) 



The next section explains how these interval numbers are used to solve global opti- 
mization problems. 



[J] 



[ Define initial search space [p] )■ ■ IPJ J 

[ Evaluate cost function: [J] I [^ 

^- ^ ppi" 

I Add to list of sub boxes | 

: ^ , L 

►I Pick first (top) box from the list 




Add to list of solutions 



I Split selected box into sub boxes | [p] 



Loop until list is empty 




[ Add to sorted list of sub boxes 
I Clip list based on J*^ 



Fig. 2 Branch and bound algorithm for interval optimization. 
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1.2 Interval Optimization 

Two key aspects of interval analysis play role in optimization: inclusion and subdivi- 
sion. The interval inclusion theorem allows an infinite set of consecutive parameters 
to be evaluated as one bounded interval, ensuring that all possible outcomes are in- 
cluded in the interval outcome. Interval subdivision, often called box splitting, is 
the process of subdividing an interval into subintervals and thereby reducing the 
dependency effect, which is the overestimation that can occur when working with 
interval numbers. The combination of division(branching) and inclusion(bounding) 
is proceduralized in the so-called interval branch and bound methods. Branch and 
bound methods form the backbone of all interval optimization algorithms |i5J. Fig- 
ure |2] shows a schematic diagram of an interval branch and bound algorithm that is 
guaranteed to find the minimum of a nonlinear cost function. 

Figure [3] shows an example of interval optimization using the branch and bound 
method. First the domain is subdivided and the cost function is evaluated, using 
interval arithmetic operations, over each sub-interval. The lowest cost function esti- 
mate p is set as the lowest upper bound of these function evaluations. Next, interval 
boxes in the domain that lead to an interval cost function evaluation with a lower 
bound higher than p can be removed from the search space. This process is repeated 
until the global minimum is found. 

Some examples of applications of interval optimization are found in the field of 
neural network optimization Q and integer ambiguity resolution||7l[8l 




Fig. 3 Principles of applying the branch and bound algorithm (hatched boxes can be elimi- 
nated based on the minimum cost function value estimate p) 



2 Human Perception Modeling 



The first application is part of ongoing research on the cybernetic approach to 
assess simulator fidelity|i9J. In order to make flight simulators more realistic, it is 
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important to know what pilots see and feel in the simulator and how they react to 
these inputs. Therefore models of pilot perception are created using pilot inputs and 
responses from both the simulator and a real aircraft (see Fig.|4|. With the informa- 
tion obtain by comparing these perception models, the motion cueing algorithms of 
the simulator can be adapted to increase simulator fidelity. 




Fig. 4 SIMONA Research Simulator and Citation II laboratory aircraft of the Delft Univer- 
sity of Technology. 

Figure [5] gives the multi-loop closed-loop control task that is used to identify the 
human perception parameters lfTOll . 



HpE Visual perception channel 
HpX Motion perception channel 
He Controlled plant dynamics 
Target signal 
Disturbance signal 
Remnant 
Pilot output 
Error signal 
Controlled state 

Fig. 5 Multi-loop closed-loop control task. 

The visual and motion channels are described by: 

H„e {j(0) =K,{\+ j(0T„) e-^"''^H„,„ {j(0) 

Hpx U(0) = (;■«) V Ho,o ijco) K,ne-i""'"H„„, {j(o) . 




(11) 

(12) 



In the visual perception channel, Hpe, Ky is the visual perception gain, Ty the visual 
lead time constant and Ty the visual perception time delay. The control action of the 
pilot is limited by the neuromuscular dynamics H„,„. The physical motion perception 
channel, Hp^^, includes the dynamics of the otoliths Hoto, the motion perception gain 
K,„ and a motion perception time delay T,„ . 

Some of these parameters can be obtained from previous research, leaving the set 
of parameters that needs to be identified as: 



[ A.y 1 y Ty Kfyi Xjn J 



(13) 
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The identification is performed in two steps. In the first step, non-parametric fre- 
quency response functions are estimated from the time domain data. The non- 
parametric frequency response functions can be estimated by using Fourier coef- 
ficients or Unear time-invariant (LTI) models (e.g. autoregressive exogenous (ARX) 
models) and are used in the second step to fit a multimodal pilot model by adjusting 
the parameters. In the second step, a comparison is made between gradient-based 
optimization and interval optimization, see Fig.|6l 



Time domain 
input/output 



ARX 



Discrete freq. ' 
response function' 



Time domain 
input/output 



ARX 



gradient 

based 

optimization 



comparison 



Discrete freq. i 
response functioni 



interval 

based 

optimization 



Fig. 6 Two-step identification. 



The cost function for this problem is: 



Continuous freq. 
response function 



Continuous freq. 
response function 



with: 



^=E 



\HpeUa))-~Hpe{jm,P)\ , \Hp^{j(o)-Hp^{j(o,P\ 



^kr 



(14) 



Hpe {j(o) Discrete frequency response for the visual channel 

Hpe {j(0,P) Parameterized frequency response for the visual channel 

Hp^ (jco) Discrete frequency response for the motion channel 



Hpx (Jft*!^) Parameterized frequency response for the motion channel 

Table [T] gives the results of the comparison between the interval branch and bound 
algorithm and the gradient-based optimization. 



Table 1 Solution of the gradient-based optimization method for the baseline condition and 
several initial starting points. 





xo(0) 


xo(l/4) 


xo(l/2) 


xo(3/4) 


^■o(l) 


Interval 


K, 


0.6058 


0.6058 


0.6058 


0.6058 


0.6058 


0.6057,0.6059] 


T, 


0.0921 


0.0921 


0.0921 


0.0921 


0.0921 


0.1518,0.1522] 


Tv 


0.2621 


0.2621 


0.2621 


0.2621 


0.2621 


0.2620,0.2622] 


Km 


0.0001 


0.0005 


0.0001 


0.0004 


0.0004 


0.0011,0.0012] 


T,„ 


0.0001 


0.6706 


0.0001 


1.0000 


1.0000 


0.3047,0.3048 


J 


24.2229 


23.0905 


24.2229 


23.4161 


23.4161 


18.3237,18.3731 



As initial condition for the gradient-based optimization, 5 different points are 
selected, denoted by xo(0) to xq{\), which means 5 points are chosen that are 
equally distributed from one side of the search space xq (0) to the other side of 
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Fig. 7 Comparison between pilot visual (//pg) channel and motion(Hp^) channel frequency 
response functions for gradient-based optimization(GB) and interval optimization(IA). 

the search space xq (1). The interval results are presented in the last column. Clearly 
the cost function J obtained with the interval methods is lower than those found 
by gradient-based optimization, which indicates that local minimums are present 
in the cost function and that the gradient method can get stuck here. Figure|2]con- 
firms these results by showing that the frequency response functions found by the 
interval method(IA) are closer to the target(ARX) than those found by the gradient 
method(GB). 



3 Aircraft Trim 

The second application concerns identification of the trim points of aircraft. Trim 
points are defined as those combinations of states and controls for which the trans- 
lational and rotational accelerations of the aircraft are zero. Analysis of the trimmed 
or steady state equations of motion of an aircraft is an important and essential part 
of the design phase, in particular for the control system. By examining the relation 
between the steady state control inputs and the flight conditions, the stability prop- 
erties of the aircraft can be determined. Finding the solutions to the steady state 
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equations of motion for nonlinear aircraft models is a global nonlinear optimization 
problem. 

The equations of motion of the aircraft model that is examined here are iflU : 

• a , ^'l^n , ^ 
u = rv — qw — g smO ^ Cx H 

m m 

V = pw — ru + gcosOsincj) -\ Cy 

m 

w = qu — pv + gcos6co&(j) -\ — —Cz 

m 



pix - i-Ixz = pqhz - qr {h - h) + qdSbQ 

leiig 



qly == pr (/z -Ix)- (p^-r^) hz + qdScC,,, - rHe, 



rlz - pixz = pq {h - h) - qrhz + qdSbCn + qH, 



eng 



where u^v^w are the translational velocities and p, q, r the rotational velocities. Con- 
verting this equation to interval notation and substituting the control and aerody- 
namic coefficients, in this case for an F-16 model, yields the following set of interval 
equations: 

[/,] = -gsin([0]) + ^ [Cx, {[a] , [5e])] + i^ [T] 

[/2]=gcos([0])sin([(^]) 

+ ^ (-3.50 • 10-4 [jS] + 1.83 ■ 10-5 [5«] +5.0 ■ 10-^ [dr]) 



[/3] = gcos([0])cos([<^]) + M (^[C2j[a])] (i - [/3f) - 1.33- 10-4[5,: 

[/4] = [Q, {[a] . m] + [4Q,4 ([«] . m)] [5a] + [AC,,s,. ([a] , [/3])] [d,] 

(15) 

[/5] = [Cm, {[a] , [4])] + {x,,,,,,f~x,.,.) [Cz, i[a])] (l - [pf ^ 

^{xc.g.ref~Xc.g.)l.33-10-'^[5e] 

[/6] = [C„, {[a] , [p])] + [AC„,5„ {[a] , [p])] [4] + [AC„,s, ([a] , [/3])] [5,] 
-f {xc.g.ref~x,,g) (-3.5 • 10-4 [/3] + 1.83 • 10-5 [5„] + 5 • 10-^ [5,]) 

Ifi] =cos([a])cos([j3])sin([0])-sin([a])cos([/3])cos([0]) 
-sin([/3])sin([0])cos([0]) 

where [/i] to [/e] are the accelerations that we try to get to zero and [fi] a kine- 
matic constraint for horizontal flight. The aerodynamic and control coefficients 
Czq,Ci,,ACi §., ACj s ,Ci„Q,AC„ s.,AC„ s are stored in data tables and an interval 
interpolation method (see fig. [Sjl is required to get these coefficients as intervals. 
Although table based data is used here, any type of aerodynamic model parameteri- 
zation can be used, for example using multivariate splines lfT2ll . as long as the values 
can be bounded by interval functions. 
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Fig. 8 Interval linear interpolation for C,„o returns an interval constructed from the lowest 
and highest value of the evaluation of the marked points. 

The parameter vector for this problem consists of the angle of attack, elevator 
deflection, thust level, sideslip angle, aileron deflection, rudder deflection, and pitch 
angle: 

[P] = ([a],[5,],m,[/3],[5„],[5,],[0]) 

The goal is to find the values of these parameters such that the total acceleration of 
the aircraft is minimized: 
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Fig. 9 Comparison between interval-trim (lA) and SQP-trim for the case of horizontal level 
flight with fixed thrust. 
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The interval branch and bound algorithm deletes parts of the search space that do not 
result in zero acceleration and therefore cannot contain a trim point. A comparison 
is made between the interval trim algorithm and the well known sequential quadratic 
programming (SQP) trim algorithm. Figure|9]gives the result of this comparison for 
the case of level horizontal flight with fixed thrust. It is well known that aircraft can 
have two distinct trim points for fixed thrust, one where the airspeed is high and 
angle of attack low, and one where the airspeed is low and the angle of attack is 
high. This latter trim point is in what is called the backside of the power curve. With 
SQP, only a single trim point is found for each thrust setting, while with the interval 
method, both trim points are found simultaneously. The SQP method relies on an 
initial guess, and by changing this guess the other trim point can be found. However, 
the problem with conventional trim methods is that it is unclear when all trim points 
are found. There might always be a different initial guess leading to another trim 
point. With interval trim this problem is resolved, since it can be guaranteed that all 
trim points are found simultaneously. 



4 Discussion on the Implementation of Interval Methods 

There are several software packages available that can perform interval operations. 
For the results in this paper the INTLAB| 13 ] toolbox for MATLAB by S.H. Rump is 
used. From the definition of the basic interval operations (eq[5ll, it can be concluded 
that interval operations require more computation effort than crisp operations, rang- 
ing from double the number of computations for addition and subtraction, up to 8 
times as many computations for multiplication and division. Additionally, the nature 
of the branch and bound algorithm can lead to an exponential growth in computation 
effort with an increase in the number of parameters, although this is not necessarily 
the case. 

As a consequence, interval optimization algorithms will require more computa- 
tional effort than their crisp counterparts. Whether to choose for an interval opti- 
mization method, depends very much on the application and the need for a guaran- 
teed solution. If the latter is more important than the computation time, then interval 
methods should be considered. As an indication of the increase in computation time, 
the results in section[3]on aircraft trim take about 200 times as long to compute with 
interval methods. On the other hand, it will take more than one attempt with non- 
interval methods to get all the solutions, and even after 200 attempts there is no 
guarantee that all the solutions are found. 



5 Conclusions 

In this paper it is shown that the system identification problem often contains a non- 
linear optimization problem. Conventional methods for solving this nonlinear opti- 
mization problem can get stuck in a local minimum of the cost function, resulting 
in a sub-optimal solution. Interval optimization is based on the inclusion property 
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of interval operations and is an excellent tool for global nonlinear optimization. In- 
terval optimization is guaranteed to find the global minimum of the cost function. 

Two applications of interval optimization are presented. From the first applica- 
tion, human perception modeling, it is concluded that interval optimization esti- 
mates the parameters of a human perception model better than previously applied 
methods. For the second application, identifying aircraft trim points, interval analy- 
sis guarantees that all trim points are found. 

Overall it can be concluded that interval optimization is a promising method for 
system identification problems that contains a nonlinear optimization. However, 
when dealing with systems that have a larger number of unknown parameters, or 
with cost functions whose evaluation requires many operations, a practical limit of 
computional effort will be reached faster by interval optimization methods than by 
crisp optimization methods. 
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Investigation of the Attitude Error Vector 
Reference Frame in the INS EKF 

Stephen Steffes, Jan Philipp Steinbach, and Stephan Theil 



Abstract. The Extended Kalman Filter is used extensively for inertial navigation. If 
initial attitude errors are small, many authors choose to represent the attitude states 
as a vector of small angles in the vehicle body frame. Some authors choose to repre- 
sent this vector in the navigation frame instead, but the corresponding reduction of 
filter performance in the closed loop filter is not discussed. Performance is regained 
when switching to an open loop filter, but closed loop filters are widely desired. 
This paper investigates this performance reduction. To show the effect, Monte Carlo 
simulation results are shown for several cases with a simplified inertial navigation 
problem using a closed and open loop filter and attitude states in the body and iner- 
tial frames. A qualitative argument is given to explain the effects, which stem from 
a state propagation model that poorly reflects the true system model for this case. 
A method is proposed to regain performance by using an estimated inertial frame 
for the attitude states. This method is only beneficial when the attitude states are 
measured indirectly via the velocity state equation. Results with this new frame are 
shown and discussed. 



1 Introduction 

Attitude determination techniques continue to be widely researched HI. For vehicle 
navigation, much of the literature defines a small angle attitude error vector (the 
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difference between the estimated and true attitude references) if the attitude error is 
expected to be relatively small. This vector is often represented in the body frame, 
but some authors choose to use the navigation frame instead. However, a discussion 
of the corresponding reduction of filter performance in the closed loop filter has not 
been seen by the present authors and is the motivation for this research. 

The choice of frame is one factor which determines the complexity of the state 
transition and state update equations and the performance of the filter. Other than 
simpler state equations, a big advantage of using an inertial navigation frame (or a 
slowly changing navigation frame) is seen in real time applications where the filter 
corrections are calculated after the reference time of the update|2|. In this case, the 
attitude error states are not rotated in the filter propagation step, making the attitude 
updates insensitive to the computational and measurement delays. In contrast, if the 
body frame is used then the attitude corrections must be transformed to the current 
body frame when correcting the attitude states. 

Attitude estimates are updated as part of the EKF update routine. In this work 
a small angle quaternion defines the error in the estimated attitude quaternion. A 
small angle attitude error vector is part of this quaternion and has additive errors to 
first order. Crassidis, Markley and ChengUJ, CrassidisQ, GraylO and MarkleyQ 
use attitude errors in the B frame, FarrellQ and Wendell'&j use attitude errors in the 
North-East-Down frame, and GaifTI, Gray0| and Thompson and Quasius|8j use 
an inertial frame. All of these authors simply add the EKF update corrections to 
the attitude error estimates, which does not change the frame of the attitude error 
vector. If the update does change the frame of the attitude error vector then the 
attitude covariance states much be rotated to the new frame. 

This paper investigates the use of a small angle attitude error vector in an inertial 
frame for an INS using the closed loop EKF. The closed and open loop EKFs are first 
presented as background for the discussion. A simple INS problem is then described 
where a small angle attitude error vector can be represented in either the inertial or 
body frames. System models, measurement models, state vectors and state transition 
equations are given for both attitude error representations. Simulation results are 
then given, which show the performance of using the closed and open loop EKFs 
in combination with both attitude error representations. The degraded performance 
of the closed loop EKF with attitude errors in the inertial frame is further analyzed. 
A new estimated inertial frame is proposed for the attitude error vector to improve 
performance of this case. Simulation results are given for this new case showing 
improved performance and are further discussed. 



2 Extended Kalman Filter 

The EKF is discussed in numerous references ll9l[l0l[TT][5j[l2l[6l and is summarized 
here. The EKF can be used in a closed-loop or open-loop manner IIT2I . The equations 
for both methods are listed in Table [T] The notation tf,- is used to denote the time 
immediately before the updates and ti^+ denotes the time immediately after. In the 
closed-loop method, corrections to the state estimates are fed back to the current 
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Table 1 Summary of continuous-discreet open-loop and closed-loop EKF equations ||9l ll2l . 
Equations on the left are only for the closed-loop filter, those on the right are for the open-loop 
filter, and the equations in the middle are for both types of filters. 

Closed-Loop Open-Loop 

System Model x{t) = f{x(t),t)+w(ty, w{t) ~ ^(0,2(0) 

Measurement Model zj- = /!j-(x(rt-)) + Vj.; /: = 1,2,... ; Vj:~^(0,fi(.) 

Initial Conditions x{0) ~ ^(x(0),P(0)) x{0) ~ ^(x(0),P(0)); §x(0) = 

Other Assumptions £ [w(f)vj] =0; V^,Vf 

State Propagation x{r) = /(x(r),f) 

Error State Propagation — 5x(r) =/(x(r),f) -/{x{r),f) 

Covariance Propagation P(t) = F{x(t),t)P{t)+P[t)F[±{t),ty + Q{t) 

Whole State Update x(r(-+) = x{tii- ) + Ki; {zi; - ht{x{t^-))) — 

Error State Update — Sx(tt+ ) = dx{ti;- ) + K/,- [z(- - fti.(x(r^.- ) + 5x{ft,- ) )] 

Covariance Update P(;t+ ) = [/ - KkHt] Pitt-) 

Kalman Gain Matrix Kt = P(tk-)Hj \HtP(tt-)H[ +Rty' 

Deflmttons F(x(r),r) = [5/(x(0,r)/5x(r)],( ,.„„ 
Ht = Htjxjti,-)) = [^/■t(xfa))/axfa)]^(,^)^j;(,^_) 



estimated state vector (x) during the update routine. The estimated state vector always 
represents the most accurate estimate of the states, which allows the most accurate 
calculation of the state estimate propagation. In the open-loop method, corrections 
to the state estimates are not fed back to the estimated state vector but are instead 
added to the current estimated error state vector (5x). The estimated state vector is 
corrected with the estimated error state vector to get the most accurate state estimates. 
This type of filter is commonly used when the state propagation calculations cannot 
be changed directly. For additive errors the error state vector is defined as: 

dx{t)=x{t)-x{t) (1) 

where x is the true (error free) state vector. Note that the Error State Propagation 
and Whole State Update equations in Table[T|all assume the errors are additive. 



3 Simplified Navigation Problem 

The following simplified navigation problem will be used in simulations to compare 
the closed and open EKF performance using an attitude error state vector in the 
body and an inertial frame. Most INS applications are much more complex than this 
example and the performance differences of the frame and filter type choices may be 
hidden by other effects. The presented navigation problem is as simple as possible 
to highlight the desired effects. 

Consider a vehicle at an Earth fixed position with an IMU providing lOOHz mea- 
surements of vehicle specific force and angular velocity in the IMU body frame (B) 
with no errors. The B frame is the rectangular coordinate system of the IMU mea- 
surements. The navigator provides the position, velocity and attitude of the vehicle 
in an inertial navigation frame (/). The / frame is defined to be equal to the Earth 
Centered Earth Fixed (ECEF) frame at time t = 0. The ECEF frame is centered at 
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the Earth's center of mass with the x-axis extending through the point (0° latitude, 
0° longitude), the z-axis extending through the spin axis and the y-axis completing 
the right-handed coordinate system. The ECEF frame rotates about the z-axis rel- 
ative to inertial at Earth rate ((OEw-th ~ 7.2921 159e — 5 rad/s). The Earth rotation 
vector in the I and ECEF frame is: 

^/^^£C£F^[0^0,a)£„,,// (2) 

Note that in the following sections the position, velocity, attitude, acceleration and 
rotation variables are a function of time. However, the (t) time dependency notion is 
dropped to simplify the notation. This notion is only used to indicate discrete time 
points or for clarification. 

The navigation algorithm calculates the vehicle state over time. The system 
model is given as llT3l[T4ll (see Shuster lfTSl for quaternion algebra definitions): 



r' = \' (3) 

v^ = r(q^)a^ + g'(rO (4) 



./ 1 / 







(5) 



where r' is inertial position, v^ is inertial velocity, q^ is the B to I frame quater- 
nion and T{qg) is the equivalent transformation matrix, a^ is the measured vehicle 
specific force, and (of^ is the measured vehicle angular velocity vector with respect 
to / in the B frame. ® is the quaternion multiplication operator and quaternions are 
represented as a column vector with the scalar element last. For this example the 
process noise w(f ) is zero for all time, g' is spherical gravity in the / frame and is 
calculated using: 

g^(r') = -Mi^/||r^||' (6) 

where /i — 398600.44 ISfcm^/i-^ is the standard gravitational parameter for Earth. 
The IMU measures the dynamics of the vehicle, which is at a fixed position relative 
to Earth. Therefore, the acceleration and rotation are constant in the B frame and: 

a^ - a«(0) = Tiqf{0)){Q' x Q' x r'(O) - ^{r' {()))) (7) 

< = <{0) - Tiqfmo' (8) 

The error states for r' and v' are additive and follow from eq. ([T]l, but the error state 
for the attitude quaternion is multiplicative. To avoid the complications with using a 
quaternion in the state vector HI a new quantity 6 will be defined, which is a vector 
of small angles representing the attitude error and is approximately additive. 6 will 
be represented later in both B and / frames, but until then the frame will be kept 
general. Consider two general reference frames A 1 and A2. The error quaternion in 
the A 1 frame can be defined as: 

p^'^qi\®(^j (9) 



Investigation of the Attitude Error Vector Reference Frame in tlie INS EKF 



349 



For small angular errors: 



,A1 



[-0^72,1] 



(10) 



where 9^^ is a small angle rotation vector in the Al frame. With this definition, the 
state vector is written as: 

x=[r^v',0^i]^ (11) 

and the entire error state vector 5x is additive. 

To derive the system model for 0^^, take the derivative of eq. (|9]): 



p^' = qA2«'qA?+q^i®a^i 



(12) 



From Savage lfT3l : 



Substituting this into eq. (fT2l) and using eq. Q and eq. (fTOl l gives: 



(13) 
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(14) 



Taking the vector part of this equation and substituting in the / and B frames for A 1 
yields the two system models: 



Al =/ : 0' 
A1=B : 0^ 



^03x1 

i -cofs X 0^ 



(15) 
(16) 



To derive F the system model must first be explicitly stated in terms of the states. 
The equation for v' must be stated in terms of 0. If Al = / then, using the Taylor 
series expansion for a small angle rotation matrix ||T6| and eq. (O, eq. ^ becomes: 



Al=/: v^=r(pOa'" + g'(r') 

«(/3.v3-(0'x) + i(0'x)(0'x)-...)a^" + g'(i^) 



(17) 



where: 



t \„B 



r(q^)a 



(18) 



350 



S. Steffes, J.P. Steinbach, and S. Theil 



Note that only the frame of a is affected by the rotation, not the length of the vector. 
One can think of a' as the acceleration in an estimated / frame called /. Finally, if 
Al ^ B then the velocity system equation instead becomes: 



Al^B:y'= r(q^)r(p«)^a« + g' (r^) 



r(q^)(/3.v3 + (0^x)-i(0«x)(0«x) + ...)a« + g^(r') 



(19) 



If Al = / then the system model for the entire state vector f{x{t),t) consists of 
eq. (O, eq. (fTSl ) and eq. (fTTl ). If Al — B then it consists of eq. (O, eq. (fT6l ) and 
eq. ( fT9] ). Calculating the Jacobian gives: 



F(x(0,r): 



03x3 ^3x3 03x3 

-M/||f'|P/3x3 03x3 Ke 



03x3 



03x3 Fee 



(20) 



where 03x3 is a 3 x 3 matrix of O's, /3X3 is the 3x3 identity matrix, and the gravity 
term is a first order approximation||5|. For Al = / the values for F^.g and Fgg are: 



Al 



(/3x3-^(0x))(a^"x) + l((a'"x0)x) + . 



/ : F,e « 

= (a^x) = ((r(q>«)x) 
Al =/: Fee =03x3 



(21) 

(22) 
(23) 



and for Al = B they are: 



Al=B: F,e ^ -T{q's) 



(/3x3-^(0x))(a«x)-i((a«x0)x) + . 



Al=B: 



-r(q^)(a«x) 
-(«/bx) 



(24) 

(25) 
(26) 



where the fact that 0'^' = O3X 1 is used to reduce the FyQ equations. 

It is known that the vehicle position is fixed relative to Earth, which is used as 
a measurement to update the filter. The velocity in the ECEF frame is then zero, 
which can be calculated in terms of the inertial states by subtracting Earth rotation 
velocity from the inertial velocity in the ECEF frame |l5i| . The measurement model 
is then: 



■Z'k 



nECEF 



{tk)y'{h-)- (r/c^^(r,)r'(f,-)) X Q'^CEF ^ 



Vk 



(27) 



where Vj '^ ,yV{0,Rk) and Tj * is the / to ECEF transformation defined as: 



rECEF 



(tk) 



COs{tk*(OEarth) sin{tk* (OEarth) 
-Sin{tk * (OEarth) COs{tk * (OEarth) 

1 



(28) 



Calculating the Jacobian gives: 
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H, = [(r2^c-£^x)r/ci^^(r,) r/ci?^ (4) 03x3] (29) 

To keep the small angle approximation in eq. (fTOl as accurate as possible, the esti- 
mated value of 0'*^ will be fed back to the whole state attitude estimate after each 
EKF update in the closed loop case. The notation tj^+ will be used to denote the time 
after the update and before the feedback, and tj^++ will denote the time immediately 
after the feedback. Therefore, immediately after the whole state update equation the 
following operations are done with the estimated states: 

qA2fe+^) = [-0^'fe+)/2,l]^®q^l(V) (30) 

0^'fe++)=O3xi (31) 



where O3XI is a 3 x 1 vector of O's, and c^2ih++) i^ renormalized to 1 after this 
operation. With this method, the estimated small angle vector 0'^' is always O3XI 
during propagation. A similar procedure is used by a number of authors||3llll and 
is often called a "reset" of the attitude states, however without the explicit notation 
used here. The effect this has on the true state 0'*^ is found by starting with eq. ^ 
at fj(.++ and using eq. (fTOl l. eq. ( l30l l and the fact that q^2(^*:++ ) ~ Qa2('a:+ ) (since the 
true attitude is not changed by the feedback) to get: 



^2 



^[-\e^\t,,)^Y®[\e^\t,,)^Y (32) 

[^\e^\t,,,), \Y^ [_ 1 0^i(r,,) + \¥\t,,), \Y 

which agrees with earlier statements that Q is approximately additive. There is no 
change in the covariance matrix due to these operations since this merely moves 
information from one place to another and does not change the statistics associated 
with these states H. Which means: 

/'(V+)=^(V) (33) 



4 Baseline Simulation Results 

Four Monte Carlo simulations are used to give a quantitative measure of the perfor- 
mance reduction when using the closed loop EKF with an attitude error vector in the 
/ frame. Using the closed and open loop EKF algorithms summarized in section |2l 
four separate simulations were run on the navigation problem discussed in section 
[3j closed loop with A 1 = B (CLB case), open loop with A 1 = B (OLB case), closed 
loop with A 1 = / (CLI case), and open loop with A 1 = / (OLI case). 

For every case, the simulation was setup as follows. The filter starts at ^ = and 
ends at t = 10. r'(0) == [6378137m,0,0]^, which is at 0° latitude, 0° longitude on 
Earth's surface. v'(0) is set to the Earth surface velocity, which is Q^^^^ x r'(O). 
q5(0) is set to a random quaternion [esin{a/2),cos{a/2)Y with a ~ ^^(0,360°) 
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Fig. 1 RMS attitude error CLI (a), CLB (b), OLI (c) and OLB (d) Monte Carlo simulations. 



and e uniformly random over the unit sphere. The estimated states are set to 
f'(0) = r(0) + v,., v^(0) =v(0) + v,„ e'^HO) = [0,0,0]^, and q^(0) =p/(8)q^, where 
V,- ~ .yK(0,CTr^3x3), Vv ^ jV {Q.Gvh-K^), p' is the quaternion [gi,g27?3,'?4]^ with 

' ^{0,Gq) and 174 = 



qi,q2,qi 



-■Iq] 



-qj- 



-qj.P{0) is set to: 



P(0) 



Cr^3x3 03x3 03x3 
03x3 ^^^^3x3 03x3 
03x3 03x3 <yghx3 



(34) 



where Gr = \m, a,, = OAm/ s, and Gq = 0.01 rad {— 4°). As previously mentioned, 
there is no system noise, so w(f ) is at all time. There is also no measurement noise 
since it is known that the ECEF velocity is exactly zero, but using zero measurement 
noise can lead to division by zero in the Kalman gain matrix. Therefore, R/f is set 
to (0.00bM/5)^/3x3. The state propagation equations are integrated at lOOHz using 
forward Euler for eq. Q, forward Euler with rotation correction! 14| for eq. &, a 
3rd order quaternion integration method 1 16] for eq. (|5]l and the exact solutions for 
eq. ( fTSl ) and eq. (fT6l l. 



Investigation of the Attitude Error Vector Reference Frame in tlie INS EKF 353 

100 simulations were run for each Monte Carlo and the same set of initial con- 
ditions are used for each Monte Carlo. Figure [T] shows the results. For each case at 
each time point, the root mean square (RMS) of the attitude errors and the corre- 
sponding Iff values for all 100 simulations are plotted. The position and velocity 
errors are all near or below their RMS 1 (7 values and are not shown. 



5 Analysis of Simulation Results 

The filter performances for the CLB, OLI, and OLB cases are all as expected; the 
RMS errors are all near or below the IGrms values. However, the performance of 
the CLI case is very poor. After the second measurement update (at 2sec) the error 
in attitude is well outside of the 1 Grms boundary. The fact that both open loop cases 
perform as expected suggests that the poor performance of the CLI case is related 
to feeding back the state corrections in the EKF update routine. The fact that both 
cases with Al — B perform as expected suggests that the CLI performance is also 
related to the state transition equations. Both the velocity and attitude propagation 
terms depend on the choice of A 1 , but the attitude terms (eq. ( l23b and eq. ( l26l )) can be 
eliminated as suspect since they do not depend on the state corrections. The velocity 
terms (eq. (l22b and eq. (l25l l) contain q^, which is corrected after every EKF update 
with eq. (l30t . It seems that the velocity terms are causing the CLI performance prob- 
lem since the attitude corrections come from updates with velocity measurements. 
The following qualitative approach explains why the CLI case inherently has poor 
performance in these simulations. 

In eq. ( l22b . a^ is first transformed with the attitude estimate into the / frame and 
this is used in a cross product matrix which is multiplied with the covariances of 0' 
during the EKF propagatiorl^. When the attitude estimate is corrected from the EKF 
update the direction of a' changes at this instant, even though the direction of a' 
did not. In effect, the / frame changes even though the true attitude does not change 
at this instant. This is misinterpreted by the EKF as a change in the acceleration 
direction relative to O', which causes poor filter performance since this does not 
reflect the system model. 

In contrast, in eq. ( [25] ). a^ is first crossed with the attitude state covariances and 
then this cross product is transformed with the attitude estimate. Again the attitude 
estimate is changed by the update, but the relationship between a^ and the attitude 
covariance (i.e. the cross product matrix) is not affected. The EKF does not interpret 
the attitude update as a change in acceleration, but it does misinterpret it as a change 
in the entire cross product. Clearly, the EKF is not sensitive to this misinterpretation 
since the performance of the CLB case is OK. 

Finally, in the OLI case the attitude corrections are accumulated in the error state 
vector instead of updating the attitude estimate. This eliminates the false accelera- 
tion direction changes in the CLI case, therefore providing better performance. 



' Additionally, note that crossing two vectors in different frames yields a resultant vector in 
an undefined frame. 
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The Monte Carlo simulations used a relatively large initial attitude error of Gg = 
4°. As this error is reduced, the size of the attitude updates is reduced and the CLI 
case shows increasingly better performance. For the presented simulation setup, an 
initial attitude error of Gg « 0.05° was found to be the boundary between good and 
poor performance for the CLI case, where "good" performance means the RMS 
attitude and velocity errors are near or below their respective IGrms boundaries. 

6 Closed Loop with A 1 = / Case 

The cross product in eq. (l22l i crosses a vector in the / frame with the covariances of 
in the / frame. Crossing vectors in two different frames in general does not make 
sense because the frame of the product is not defined. A better approach might be 
to use some 6' as the attitude state instead. To show how this simple, yet unconven- 
tional change affects the EKF performance, the system model and update equations 
will be derived and Monte Carlo results for the resulting system will be shown. 
To start, 6' is first defined as: 

e'^Tiq'B)Tiqf)e' (35) 

Using eq. ^ this becomes: 

e'^T{{p')-^)e' (36) 

This can be further reduced using a Taylor series expansion to: 

e' ^{h,i + {e'x)-^-{9'x){e'x) + ...)e' (3?) 

e' w e' (38) 

It is already known that 6' (and thus p^) are constant during the EKF propagation 
from eq. JTSl . Therefore, the system model for this state must be: 

e' = 03xi (39) 

To find the system model for velocity combine eq. ( fTTl l and eq. ( l38l l to get: 

y'~ih.3^ie'x) + ^{e'x)ie'x)-...W + ^{r') (40) 

Therefore, the F matrix for this case is the same as in the CLI case. Additionally, 
the measurement equations are unchanged since they are not affected by the choice 
ofAl. 

To keep small angle approximations as accurate as possible, the estimated value 
of 0^ will be fed back to the whole state attitude estimate after each EKF update, 
analogous to the procedure in eq. ( l30l l and eq. d^Tb . Immediately after the whole 
state update equation the following operations are done: 
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^sih^^) - h0V)/2,l]^®qU?^O (41) 

0V+)=O3xi (42) 

As a result, the / frame changes discretely at the update times. The resulting change 
in Q^ is calculated by starting with eq. ( l36l ) at tj^++ and using parts of eq. ( l32l ) and 
eq. ( l36l ) again to get: 

e'{t,,,) = T{{,?'{t,,,))-')d'{t,,,) 

^T{[-\^'{t,.)^Y){T{[\0^{t,.)^Y)0'{t,.)-T{[\e^{t,,)^Y)e'{t,,)) 
^T{[-'^e'{t,,),\Y){e'{t,,)~e'{t,,)) 

(43) 
The last equation shows that Q^ is not additive because the / frame changes by 
the rotation r([— 56^ (?,(.+ ), 1]^) due to the feed back operations. In this case the 
covariance matrix must also be updated to reflect the frame change. Therefore, as 
a final step in the update routine, a discrete propagation of the covariance matrix 
must be done from ti^+ to 1^^++ to change the frame of the attitude covariances. The 
discrete form of the EKF propagation equations in Table [Tl are Il9l fT2ll : 



F, 



k+ 



x(fi++) =/(x(fi+)) 


(44) 


P{tk^^)=h^P{tk^)F^^+Qk^ 


(45) 


{t{t,,)) = [5/(x(f))/ax(r)]x(,)=x(r,,) 


(46) 



The / function does not change r^ or v^, but uses eq. (l43l l for the attitude states. 
From the Jacobian of eq. ( |43] ). the system matrix is: 



f'k+ 



hy3 03x3 03x3 
03x3 ^3x3 03x3 
03x3 03x3 r(h^0%+),l]^) 



(47) 



and the process noise is Qi^+ = 0. The EKF update is finally complete with these last 
steps. 

A Monte Carlo of 100 simulations, like those discussed in section 21 was done 
for the closed loop A 1 = / case to give a quantitative measure its performance. 100 
simulations were run and the same set of initial conditions from section |4| are used. 
Figure|2]show the results. As with the other simulation results, the root mean square 
(RMS) of the velocity errors, attitude errors, and their corresponding 1 a values for 
all 100 simulations are plotted. The position errors are not shown because they are 
approximately an integral of the corresponding velocity errors and are therefore not 
interesting. 

The filter performance for this case is similar to the CLB, OLI and OLB cases. 
The RMS errors are all near or below the 1 (Jrms boundaries. 
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Fig. 2 RMS attitude error for closed loop EKF with A 1 = / Monte Carlo. 

7 Conclusion 

The presented work has shown how the choice of the attitude error vector frame 
and filter type can effect filter performance. Four separate Monte Carlo simulations 
were done for a simplified navigation problem. The filter performance for the CLB, 
OLI and OLB cases was as expected, but the performance of the CLI case was 
relatively poor. In all cases, the attitude corrections were estimated indirectly via the 
velocity measurements with the F^.q term in the EKF propagation equation. In the 
CLI case, the af term in F^q changes after the EKF update but the attitude covariance 
states are unchanged. The poor performance of the CLI case can be attributed to this 
inconsistent modeling. 

To avoid the problems with the CLI case, the / frame was used to represent the 
attitude states instead. This case has the same propagation equations as the CLI case, 
but uses an additional discrete filter propagation step immediately after the normal 
EKF update to rotate the attitude covariance states to the new / frame. In this case 
both the a' term in F^g and the attitude covariance states are always in the current 
/ frame, which fixes the inconsistent modeling problem from the CLI case. Monte 
Carlo results for this case show similar performance to the CLB, OLI and OLB 
cases. 

Most INS systems are much more complicated than the example used in this 
work. However, if a filter like the CLI case is used then the inconsistent modeling 
issues discussed in this work may degrade performance. For any system, all options 
should be considered to find the best choice for the application. 

References 



1. Crassidis, J.L., Markley, F.L., Cheng, Y.: Survey of Nonlinear Attitude Estimation Meth- 
ods. Journal of Guidance, Control and Dynamics 30(1), 12-28 (2007) 

2. Gray, C.W.: Star Tracker/IRU Attitude Determination Filters, vol. 107, pp. 459^76 
(2001), AAS 01-039 



Investigation of the Attitude Error Vector Reference Frame in tlie INS EKF 357 

3. Crassidis, J.L.: Sigma-Point Kalman Filtering for Integrated GPS and Inertial Naviga- 
tion. In: AIAA Guidance, Navigation, and Control Conference, San Francisco, CA (Au- 
gust 2005), AIAA-2005-6052 

4. Markley, F.L.: Attitude Error Representations for Kalman Filtering. Journal of Guidance, 
Control and Dynamics 26(2), 311-317 (2003) 

5. Farrell, J.A.: Aided Navigation: GPS with High Rate Sensors. McGraw Hill, New York 
(2008) 

6. Wendel, J.: Integrierte Navigationssysteme: Sensordatenfusion, GPS und Inertiale Navi- 
gation. Oldenbourg Wissenschaftsverlag, GmbH (2007) 

7. Gai, E., Kevin Daly, J.H., Lemos, L.: Star-Sensor-Based Satellite Attitude/Attitude Rate 
Estimator. Journal of Guidance, Control and Dynamics 8(5), 560-565 (1985) 

8. Thompson, I.C., Quasius, G.R.: Attitude Determination for the P80-1 Satellite. In: Pro- 
ceedings of AAS Guidance and Control Conference (1980), AAS 80-001 

9. Gelb, A. (ed.): Applied Optimal Estimation. The MIT Press, Cambridge (1974) 

10. Brown, R., Hwang, P.: Introduction to Random Signals and Applied Kalman Filtering. 
John Wiley and Sons, Chichester (1997) 

11. Grewal, M.S., Andrews, A.P: Kalman Filtering - Theory and Practice using MATLAB, 
2nd edn. John Wiley & Sons, Chichester (2001) 

12. Kayton, M., Fried, W.R.: Avionics Navigation Systems, 2nd edn. John Wiley and Sons 
Inc., Chichester (1997) 

13. Savage, P.G.: Strapdown Inertial Navigation Integration Algorithm Design Part I: Atti- 
tude Algorithms. Journal of Guidance, Control, and Dynamics 21(1), 19-28 (1998) 

14. Savage, P.G.: Strapdown Inertial Navigation Integration Algorithm Design Part 2: Veloc- 
ity and Position Algorithms. Journal of Guidance, Control and Dynamics 21(2), 208-221 
(1998) 

15. Shuster, M.D.: A Survey of Attitude Representations. Journal of the Astronautical Sci- 
ences 41(4), 439-517 (1993) 

16. McKern, R.A.: A Study of Transformation Algorithms for Use in a Digital Computer. 
Master's thesis, Massachusetts Institute of Technology (January 1968) 



Nonlinear Filtering Using Sparse Grids 

Carolyn Kalender and Alfred Schottl 



Abstract. This paper presents a new nonlinear filtering algorithm applicable in real- 
time. Nonlinear filtering problems are mostly solved with the Extended Kalman Fil- 
ter which due to the nonlinearities is a suboptimal estimator. Optimal estimates are 
provided by Fokker-Planck-Equation in combination with Bayes rule. Conventional 
approaches for the numerical solution of this equation suffer from the "curse of 
dimension" and are therefore not applicable in higher dimensions. We use sparse 
grids for solving the Fokker-Planck-Equation and present a six dimensional nonlin- 
ear problem solved in real-time with this new approach. 

1 Introduction 

The estimation of the course (and further states) of a manoeuvring target based on 
measurements by one or more sensors such as radar stations are considered. This 
problem plays an important role in various applications such as in the guidance of 
an interceptor missile against an incoming threat. It is well-known that a precise 
estimation is crucial for the miss distance or, equivalently, the hit probability. Most 
problems of this kind are highly nonlinear. 

Let {Q,JF,P) be a probability space endowed with a right-continuous filtration 
(J^t) and let W and V he ad- and m-dimensional adapted Brownian motion. 

The object's motion is modelled by an adapted stochastic process X = (X,), X; £ 
W' , the dynamics of which are given as the strong solution of a nonlinear stochastic 
differential equation 

dXr=fr{Xi)dt + aiiX,)dW, . (1) 
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In the continuous time setup, the measurement can be modelled by another stochas- 
tic process Y, again defined (up to versions) as a strong solution of the stochastic 
differential equation 

dYr=gr{Xr)dt + Vr{X,)dV, . (2) 

It is well-known (see fTll, chapter 8.6) that under appropriate growth conditions such 
as (q denotes any of the measurable functions /, a, g, v) 



\\q,{x)-q,{x')\\'<K\\x^ 



- X 



/||2 



\\q,{x)\\'<K{\ + \\xf) 

a strong solution exists. 

As we will see, the filter may also be applied at the discrete update times 

dY,^^h,{X,^) + v,^{X,^)V„ . (3) 

By interpreting 0]) as a system equation and (|2]) as a measurement equation, the 
problem of estimating the actual state Xt of the object only using measurements Ys<t 
can be seen as a filtering problem: Let ^J be the filtration generated by Y . We 
are considering the problem of finding an optimal (in the ^^ sense) ^^ -adapted 
estimation of X. It can be easily seen that this problem is equivalent to finding the 
conditional expectation £ (X, | J^, ) . 

There are various ways of approaching such a problem. The most usual method 
is to apply an extended Kalman filter (see e. g. jTOl ). a method suitable and highly 
efficient for systems with modest nonlinearities. Since the extended Kalman filter is 
based on linearisations of the system equation, divergence is possible. In addition, 
nonsymmetric or multimodal distributions cannot be treated since classical Gaus- 
sian theory is applied. 

Another widely used method is the particle filter (see e. g. ifTOll ) which uses a 
reasonable amount of state samples and propagates them through simulation of the 
system. The particles which may be viewed as a discrete distribution approximating 
the conditional probability distribution, are assigned normalized weights. An up- 
dated approximation is generated by changing the weights with respect to the mea- 
surements (e. g. using Bayes formula) . The quality of the approximation is heavily 
dependent on the number of particles. As a rule of thumb, the necessary number 
of particles grows exponentially with the number of dimensions of the system. This 
"curse of dimension" (see ID) restricts the usage of particle filters to relatively small 
dimension numbers. In typical applications however, the dimension d of the sys- 
tem state is relatively high (5-10), while the dimension m of the measurement is 
moderate (3-6). 

It is furthermore well known (e. g. |l6|) that only in very special setups it is pos- 
sible to calculate the estimate in a closed form with a finite-dimensional system 
of equations. Most importantly is the linear case with the Kalman filter which just 
needs to update d conditional expectations and ^ ^ covariances. Since the linear- 
ity implies Gaussian conditional distribution, the complete distribution is specified 
by these parameters. Other so-called finite dimensional filters are the Benes |23 and 
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the Daum filters pl, which are of upmost theoretical interest but require artificial 
and restrictive conditions on / and g. In the vast majority of cases it is necessary to 
consider an infinite amount of numbers to specify the whole conditional distribution 
of (X\J^^) or to be satisfied with only approximations. 

We are assuming that the conditional probability density function (pdf) pt 

p,{x) = ^P{X,<x\.^J) , 
ax 

which is a measurable function of (f ,x), exists. 

The analysis of the evolution of the conditional distribution is part of the general 
filter theory. Under very mild assumptions, filter formulas such as the Kushner- 
Stratonovich equation (see |lj Theorem 3.30) have been developed. The Kushner- 
Stratonovich equation is equivalent to a stochastic partial differential equation for 
the pdf if the solution of the differential equation exists (see ||7|, Theorem 8.6). For 
details about the existence of the conditional pdf see also Theorem 7.11 in [IJ. A 
thorough analysis of the conditions and properties of the solution is contained in IHl . 
It holds 

dtPt{x) = -X^~(/aWAW) + :7E 3 3 {bt.jk{x)pt{x)) dt 

y it OXk / ji^ OXjOXlf j 

+p,{x) {h,{x)-E{h{Xr)\.^J)) {d\-E{h{X,)\.^l)dt) 

with bt = Of oj ■ 

The right-hand side of this equation may be seen as the sum of a propagation part 
(the first line), containing a transport (or advection) term and a dissipation (or diffu- 
sion) term, and the innovation part (the second line) which handles measurements. 
While the transport term shifts the pdf according to the model, the diffusion term 
widens the pdf in time which inserts uncertainty into the estimation. The measure- 
ment term will in turn narrow the pdf due to the measurement update. 

The discrete time case is similar. The innovation part (which can be seen as an 
abstract version of the Bayesian rule) is simply replaced by the classical Bayesian 
rule such that the propagation is done via the partial differential equation 

dtPM) = [-"L^^f'Mpt{^)) + ^E ^4-(^''^W^' W) 1 ^' (4) 

V k '■' ik OXjOXji^ j 

while measurements y at time t are incorporated separately via 

+ , , pT {x)pY\x{yV) 
pT{x) = .,._.., ■ (5) 

i PY\x{y\z)Pt [z)dz 

Herein p^ denotes the conditional pdf of Xt just after the measurement at time / 
has been considered, pf' denotes the pdf just before the measurement. For small 
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measurement time intervals the discrete time and the continuous time formula be- 
come equivalent. In the sequel, we are considering the discrete time formulation. 

2 Discretization 

Conventional approaches for solving partial differential equations numerically suf- 
fer from the "curse of dimension". To achieve a given order of approximation the 
number of grid points grow exponentially with the dimension such that 

||/-/„|| = ^(«-'-A0 

for a function / of smoothness r. 

The dimension of the filtering problem ranges typically from 5 to 10 and is there- 
fore out of reach for real-time applications with regular grid methods. Even the use 
of an adaptive grid as shown in 1 1 3 1 for only 4 dimensions could not be implemented 
in real time. However, the technique of sparse grids offers a possibility to drastically 
lower the number of necessary grid points from ff{N'') to i)'{N{\ogNy'^^). 

We suggest the use of sparse grids to make the full nonlinear problem in higher 
dimension treatable in real-time. The algorithm is split up into three parts. First 
the density function is propagated by solving equation Q on sparse grids. Next, the 
density is updated with the actual measurements and then finally the expected values 
or other characteristic values (e. g. higher moments) of the conditional distribution 
are extracted from the sparse grids. 

2.1 Sparse Grids 

Sparse grids were first introduced by Zenger llT2l and have in the meantime been 
widely used e. g. in the area of finance mathematics. 

The basic idea is to decompose the space of piecewise multilinear functions in its 
hierarchical subspaces and then consider only those for which their contribution to 
the interpolation of smooth functions is significant. 

For the multilinear basis functions 

where the gridwidth h/ = 2^'j and the related grid points x/ , with x/ , = ij -hi. on 
the level / with the index ;, the space of piecewise multilinear functions of level / in 
the interior of [0, 1]'' is given by 

Vi ^ spcin{(j)ij : 1 </<2'-l}. 

The hierarchical space 
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Wi^V,\Q Vi^e, = span{0,,,- : 1 < / < 2' - 1 , /;■ odd V;} (^ V; = Wk) 

i=l k<l 

is given as a set difference from a coarser to a finer grid. Hence for a functional 
decomposition 

Li 

the hierarchical surplus m/ ,■ contains the difference in x/ , between the relative coarser 
and finer interpolation. While the number of grid points increases considerably with 
the level, it turns out (see Zenger lfT2l ) that the gain in interpolation accuracy is get- 
ting comparatively small for smooth functions. The idea of sparse grids is instead 
of using a full grid with 0ui <iW]t only to use the lower level hierarchical sub- 
spaces and form ®\kU <L+d- 1 ^k (a tetraeder of subspaces is composed instead of a 
quader). L is called the level of the sparse grid. It should be noted that no grid points 
are located at the boundary of the domain. 
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Fig. 1 Hierarchical subspaces W/ , |/|«, < 3 and sparse grid of level L = 5 

Instead of ^{N'^) grid points in the regular grid, the sparse grids contain only 
6'{Ni\ogNY'^^) grid points (A^ = \/h = 2^) by having only a slightly smaller order 
of interpolation accuracy for smooth functions in the maximum norm (respectively 
identical order of interpolation accuracy in the energy norm, for details see e. g. lO). 

Due to these promising properties of sparse grids we will use them in the sequel 
for solving the filtering problem numerically in real time for higher dimensions. 



2.2 Propagation 

Finite differences are used to discretize the propagation equation ^. The advection 
part, especially for highly agile targets, forces the localized density to move with 
time across a large region in the state domain. It would imply unnecessary high 
computational effort to discretize the whole region at every time step. As we will 
see, this can be avoided by introducing grid-tiling and grid-drifting. 
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2.2.1 Finite Differences 

The following finite difference scheme is proposed 

• A first order forward scheme for the time derivative 

r,+ Pt+At — Pt Opt ^ 1^/4 ,\ 

DJPt = j-^ , -^ = D^pt + e{At) 

• Upwind differences for the advection part 



p,(x+Ax)-p,(x) fcr f(Y\^r\ T 

P,(..)-y,(..-^.,) fo, /,(.,)> [I i)x 
• Central differences for the diffusion part 

Ax-'- dx^ 

Mixed derivatives are not regarded here but could also be realized by finite differ- 
ences. In contrast to regular grids however, there is not a natural grid neighbour in 
mixed directions but one can interpolate between different points. For usage pecu- 
liarities of finite differences on sparse grids see [9j. 
All together this gives an explicit first order scheme: 

P,+A,{x) = P,{x) ~ AtJ^f,,,{x)Dlp, + ^-AtJ^bkkD\p, (6) 

As an explicit scheme it has a timestep condition to be numerically stable. For 

the following theorem holds 

Theorem 1. The discretization scheme (|6|) for which holds is stable according 
to the von-Neumann stability analysis. 

Proof. Apply the argumentation of lITTI p. 160] to the multidimensional case. 

2.2.2 Tiling 

Tiling helps in dealing with pdfs which move and widen due to drift and diffusion 
with time. Also Tiling restricts the computational effort to a possibly small subset 
of the state domain with a probability close to 1 . 

In contrast to regular grids it is not natural to expand sparse grids just by adding 
a few rows of grid points since this would contradict the hierarchical structure of 
those grids. If we want to add or delete grid points we do so by adding or removing 
an entire sparse grid. 
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To represent a more general area, we cover the relevant region of the domain 
with tiles each containing a sparse grid (which does not contain boundary points). 
Between the sparse grid tiles we have to add boundary layers as a connection. Those 
boundary layers are sparse grids themselves with a lower dimension. 

A tile is removed if the integral of the pdf on this tile is below a given threshold. If 
on a boundary strip the integral part of the pdf is over a given threshold, a neighbour 
tile is then added. 







Fig. 2 Tiling examples in two and three dimensions 

In regular intervals it is checked if still every tile is necessary and if new tiles are 
required. 

2.2.3 Drift Compensation 



As stated in section lZ2.ll the time stepsize must be limited in order to guarantee the 
stability of the scheme. Theorem [T] states that the condition ^ must be fulfilled for 
a stable scheme. Note that the magnitude of the drift |/,jt| determines the stability 
margin in ([7]). 

In applications, a huge drift term in some dimensions (e. g. the position states in 
a tracking application) is likely. Fortunately, the pdf is often well-localized for these 
dimensions. A significant improvement can be achieved by modification of the drift 



dX, = (/, {X,)+c)dt + a, {X, ) dW, 



(8) 



with an appropriate choice of c. Obvious choices are c = E{ft{X,)\^l) or c 

MEix,\^n)- 

Rearranging the advection term part of equation ^ results in 



d,p,{x)+^Ck-—p,{x) = -^^—{ft.kix)pt{x)) dt 



or, discretized by the upwind-scheme, 



Pt+At{x + cAt) = p,{x,t) - At^f,_i,{x)D:,i^p,{x) 

k 
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The spatial shift from x to x + cAt is reaUzed by introducing a shift vector v which 
is attached to the grid. The numerically treated advection term is just the difference 
in drift /, = /, — c which should be absolutely smaller than the original drift. This 
has an influence on the stability behaviour: a significantly larger step size is then 
feasible. 

In the implementation, the attached shift vector v is modified as soon as the cal- 
culated stability margin falls below the threshold. 

2.3 Measurement Update 

The pdf is updated using measurements according to Bayes' rule (|5]l. The multipli- 
cation is simply done for every sparse grid point. The denominator is computed by 
a piecewise linear interpolation of the integrand which gives a coarse norm value of 
the updated pdf. 

2.4 Expected Values 

Usually, we are not interested in the whole pdf but only in its certain characteristic 
properties. For most cases it is reasonable to extract the expected values 

Ex = / xpt {x)dx. 

Instead of approximating the integrand with piecewise constant or linear functions 
as it is done in the normalization of Bayes' rule, the pdf itself is piecewise interpo- 
lated using Gaussian densities in every coordinate direction. With 




the interpolation points {x,y) = (x/,p,(x,)), {xj,pt{xj)) and {xk,Pt{xk)), and build- 
ing the quotients 

p,(Xj) hik 

we obtain for three neighbouring grid points x, = xq, xj = xq + Ax and a^ = xq — Ax, 
the parameters 

M=xo + -Ar ■' , o'- = ~2- 



2 " l+r,yi hij l + njk 

and accept this interpolation for a^ > and good conditioned values of ju. All to- 
gether we find the expected values by averaging the piecewise values of ^. 
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3 Algorithm and Illustration 

The algorithm consisting of the parts described above can be summarized as follows: 
Algorithm 1. Filtering algorithm 



Initialize sparse grid structure and pdf 
for every time step do 

Compute drift compensation c and shift vector 

Solve Q with difference drift / using finite differences 

if measurement available then 
I Measurement Update 

end 

Normalization 

Compute expected values 

if Tile Adapt Time then 
I Adapt tiles 

end 
end 



For illustrating the algorithm, it is useful to apply it to a two-dimensional ex- 
ample. As a model problem we take the undamped pendulum with the following 
system equations 

Xl = X2 

X2 = —c- sin(xi ) 
and the measurement equation 

with the states angle and angular velocity: xi ~ (p, X2 ~ <p- 

Figure [3] shows contour plots of the pdf at different time instances. The x- 
axes ranges from (p = 2.73 rad to 4.69 rad, the y-axes from (p = —0.38 rad/s to 
3.56 rad/s. The Gaussian distribution at f = 0.0 s is initialized with jj. = {n,0.45)^ 
and (7 = (0.1,0.2)^. Afterwards, the pdf moves along its states with time according 
to Q. The position of the tiles is marked with squares, the interconnecting layers 
are depicted by lines. The grid drift, addition and removal of tiles can be observed 
from one time step to the next. Measurement updates were performed at r = 0.3 s 
and t = 0.6 s. 
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Fig. 3 Evolution of the pdf for the model problem 



4 Example 

To show the performance of the sparse grid tracking filter a target with high nonlin- 
ear dynamics is considered. In fSl a guidance for an anti-ship missile is proposed 
composed of a PN plus a barrel-roll distraction manoeuvre. The target dynamics can 
be described with the following nonlinear state equations: 
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xi = —v,„ 008X5008X4 

X2 = 8inX5 

XI 

Vm 

X3 = C08X5 8mX4 

X\ CO8X2 

Vm8inX4 . 8inX5COSX4 

X4 — (1 —A' — tanx2 8mx5 CO8X4CO8X5) — CO 

XI CO8X5 CO8X5 

X5 = — ((1 — Af)8inx5C08X4 + tanx7 8in X4CO8X5) +ft)8inx4 

X\ 

with the 8tate8 describing the distance xy — r, the line of sight angles X2 = 0l, -^3 = 
1//L and the angles of the missile velocity with respect to the line-of-sight X4 = 1/4^, 
•^5 = Qm- There are three parameters in the model: the value of missile velocity 
Vm, the coefficient of the proportional navigation A^ and the angular velocity of the 
barrel roll CO. The dimension of the system ranges between five and eight depending 
on whether the parameters are known in advance or not. The results shown represent 
the case with an estimated CO with CO = and we have therefore a six dimensional 
system. 

A radar station shall measure the range and LOS angles 



with 

'100 

0.004 

0.004y 

at a frequency of 5 Hz. The system noise is given by 

a,, =diag(10, 0.3, 0.3, 0.6, 0.6, 1). 

The initial state values are assumed to be known except for the noise with a standard 
deviation of 

a = diag(100, 0.004, 0.004, 0.1, 0.1, 0.25). 

The missile system angles )/% and Qm are only very poorly observable. These states 
diverge using an Extended Kalman Filter due to the strong nonlinearities. Simulation 
results computed in real time in comparison to an EKF are shown in figure|4]for y/^ 
and 1/%. The improved behaviour of the sparse grid filter is obvious and confirmed 
by 100 Monte-Carlo runs. Figure|5]shows the RMSE in i///, and i/%- The divergence 
of the EKF in i/% causes also bad estimates in the observed states as can be seen 
from the RMSE for i///,. The EKF tends to diverge and depends strongly on the 
measurement update while the sparse grid filter shows a good performance. 



370 



C. Kalender and A. Schottl 





Fig. 4 Simulation results for I///, and ^m 





Fig. 5 RMSE for y/i and (|/m 

5 Conclusions 

This paper presented a new approach for nonlinear filtering. The full nonlinear fil- 
tering problem is solved by a probability density function in which the time evolu- 
tion is determined by a partial differential equation for prediction and Bayes' rule 
for measurements. The technique of sparse grids was used for the discretization of 
the continuous problem and provided an algorithm applicable in real-time also for 
higher dimensions. The discretization was done by finite differences on the sparse 
grids for which tiling and grid drifting to increase efficiency and accuracy was in- 
troduced. The estimated state values were extracted using piecewise interpolation 
with Gaussian densities. The performance of the new algorithm was shown on a 
six dimensional nonlinear problem in comparison to an EKF. The algorithm was 
applicable in real-time and showed a better performance than the EKF. 
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Observability of Star Tracker / Gyro Based 
Attitude Estimation Considering Time- Variant 
Sensor Misalignment 



Stefan Winkler 



Abstract. The fusion of measurements from star trackers and gyroscopes within 
optimal estimators/filters is a common approach for spacecraft attitude determina- 
tion. For applications where filter tuning is not sufficient to account for unmodelled 
deterministic errors, state augmentation is often the method of choice. So also here, 
where the focus is on deterministic time-variant misalignment between star tracker 
and gyroscope unit as this often occurs in missions with repetitive ecplipse and sun 
phases. Based on the derived filter dynamics and measurement equations, an observ- 
ability analysis is performed. Different practical cases are distinguished to analyze: 
(1) which filter states are observable, (2) which only in linear combination and (3) 
which not at all. 



1 Introduction 

Information on the spacecraft attitude is essential for almost every space mission. 
A comprehensive survey of the most promising nonlinear methods that have been 
developed during the last 25 years is given in (25. This paper here focuses on sensor 
data fusion of star tracker (STR) and gyros, also known as gyro-stellar estimation. 
Star trackers and gyros have a number of complementary properties which makes 
them interesting for sensor data fusion. E.g. while a star tracker provides low- 
frequent long-term stable attitude measurements, the attitude computed from the 
high-frequent gyro measurements is only short-term stable. The motivation to fuse 
star tracker and gyro is to generate a long-term stable attitude estimate at gyro mea- 
surement frequency of attenuated noise compared to the star tracker measurement 
and, hence, to optimally bridge periods where no star tracker measurement is avail- 
able. In addition to the improved attitude information, the estimator provides an 
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estimate of the gyro angular rate bias. The typical extended Kalman filter has 6 
states: 3 attitude errors, 3 gyro bias errors. For details see e.g. |[Tl|2l[5]|6l. 

Misalignment between star tracker and gyro unit and error in gyro scale factor al- 
ways occur. If the spacecraft angular rate vector is constants w.r.t. to the spacecraft, 
the filter will estimate constant misalignment and scale factor error as part of the 
gyro bias estimate. Hence, when the 6-state filter is in steady-state, both constant 
misalignment and scale factor error do not influence the attitude estimate anymore. 
Performing hereafter an attitude maneuver, where the angular rate vector is not con- 
stant anymore, will corrupt the attitude estimate. This fact is important for agile 
spacecraft that require high-precision attitude information. 

Time- variant misalignment between star tracker and gyro unit is often caused by 
temperature changes between sun and eclipse phases during orbit. It can have a sig- 
nificant impact on the achievable attitude estimate performance. Hence, it is usually 
tried to limit it by specific constructional design, e.g. 131 [TOl flTi . The remaining 
time-variant misalignment can be considered in the attitude estimation filter. 

While time-correlated star tracker measurement noise can be well considered by 
appropriate filter tuning^, the attitude estimate accuracy degradation due to (deter- 
ministic) time-variant star tracker / gyro unit misalignment can not sufficiently be 
compensated with this method for high-precision attitude determination. It is rather 
useful to augment the common 6-element filter state vector by the time-variant mis- 
alignment states ll9l fT0l . This paper focuses on the observability of such state aug- 
mented filters. 



2 Attitude Difference Differential Equation 
2.1 Attitude Representations 

This section summarizes the attitude representations, their nomenclature and re- 
lations as used throughout this paper. For a thorough review see [SI and for one 
regarding attitude estimation with Kalman filtering 16] . 

2.1.1 Quaternion 

An attitude or unit quaternion, q, here shortly denoted as quaternion, describes a 
single-axis rotation about the Euler-axis (unit vector e) about the rotation angle i> 



q = 



esin Y 
cosf 



(1) 



where q is its vector element and (74 its scalar element. A quaternion obey the unit 
length constraint |q| + ql ~ 1. The quaternion q2i describes the rotation from an 
1- into a 2-frame and, hence, the attitude of a 2- w.r.t. an 1 -frame. 



In amplitude and direction. 



^ Adaptation of the measurement error covariance matrix in the filter. 
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2.1.2 Gibbs Vector 

The Gibbs vectoo g, is defined using quaternion such that 

g = q/li = etan(V'/2) == a/2 



(2) 



where a is used throughout this paper as Gibbs vector representation. It clearly is 
equal to twice the Gibbs vector g. For small rotation angles -0, one can write 



e^A = [-0X V'y '^zY 



(3) 



where ip^, ipy and ipz is the rotation angle about the x-, y- and z-axis, respectively. 
Hence, a becomes the rotation vector. 

In this paper, the vector a2i describes the rotation from an 1- into a 2-frame and, 
hence, the attitude of a 2- w.r.t. an 1 -frame. 

2.1.3 Direction Cosine Matrix 

Considering a sequential 3-2-1 rotation about the small angles 5>F, 50 and (5<?, re- 
spectively, leads to the direction cosine matrix 



T = 1 5T 

where ST is a skew-symmetric matrix, namely 



5T 



-5^ 50 

S^ -6^ 

-50 5<P 



5<P 
50 
5W 



5ct)'' 



(4) 



(5) 



A direction cosine matrix T21 describes the rotation from an 1- into a 2-frame and, 
hence, the attitude of a 2- w.r.t. an 1 -frame. A vector given in components of an 
1-framce, vi, is transformed into a 2-frame, V2, by V2 = T21V1. 



2.2 Theoretical Derivation 

Considering the attitude between body- and inertial frame (subscript b and i, resp.), 
the attitude kinematics equation using quaternions is given by 



q;,,: 



Ab 



'Qbi 



- ^l^ ® q6, 



(6) 
(7) 



where uif' is the angular velocity of b- w.r.t. i- in the 6-frame and is the quaternion 
multiplication introduced in Q. 



' Also known as Rodrigues parameters. 
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Analogous, the attitude kinematics between a second frame (subscript s) and the 
inertial frame is 

cist^ --^l' (E)qsf (8) 

For the attitude difference quaternion, q;,,, , which will reflect an attitude error later 
on in this paper, one can write 



The time-derivative of this attitude difference is 



(9) 



<ibs = q.bi ® Qs,^ + qbi ® Qs/- 



(10) 



Using the attitude kinematics equations from above leads to 



Qfts 



' q&i ® Qs 



qbi 



•qbs 



-q,,s ® tcr. 



(11) 

(12) 



Introducing the Gibbs vector representation ab^, which corresponds to q^s, it fol- 
lows 



afes = 



1 



I - ia,. 



2 



I - l^bs 



li-bs 

2 



)-ro. 



I + -afe.aft, 



w,, -a;„ - - w. 



.ib 



w, 1 X afes 



(13) 
(14) 



From now on, only small angular differences between s- and 6-frame are assumed. 
Hence, af,^ is the small angular difference between both frames. Using the common 
small angle simplifications (sin?/; « V> cosip w 1, ';/;■!/; « 0) leads to absa. 
Hence, Eq. (fT4l l becomes 



r. = 0. 



a,, = (u;f - O - i (u;f + O X a^.. 



(15) 



This is the attitude difference equation in Gibbs-vector notation for small attitude 
differences between b- and s-frame. 



2.3 Application to Star Tracker I Gyro Fusion 



A problem often considered in practice is the determination of the attitude of the 
body-frame w.r.t. inertial-frame. This attitude shall be expressed by the quaternion 
qbi. If ijj'^j^ is known, starting from an initial attitude, the attitude vs. time can be 
determined by integrating the kinematic equation (Eq. (|6|). In reality usf is not 
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exactly known but it can be measured with an inertial measurement unit (IMUjj. 
With the u-frame as the IMU-(fixed-)frame (subscript u), the measurement provided 
by the IMU can be modeled as 

'^r - '^r + b„ + w„ (16) 

where b is the deterministic error and w the stochastic error on the IMU measure- 
ment. 

If there is an angular velocity between IMU- and body-frame, a;''", which is the 
considered time-variant misalignment, one can write in more detail 

cD»=u;,f + u;^ + b„+w„. (17) 

Obviously, there will be a difference, an error, between the attitude of body- w.r.t. 
inertial-frame and IMU- w.r.t. inertial-frame. The linear approximation of this atti- 
tude error in Gibbs- vector notation is given by Eq. dTSt . If this error, namely a^s, is 
known, the attitude obtained from integrating IMU measurements can be corrected. 
Hence, the goal must be to determine ab^. For this, Eq. (fTSl l can be integrated. The 
expression for the ideal angular rate of the body- w.r.t. inertial frame, ujI'^, is given 
using Eq. dTTI ) by 

u;f - Tb„ (cD™ - u;t" - b„ - w„) . (18) 

In general, the term a;*'' in Eq. (fTSl l could be the angular rate measured by the 
IMU. But to keep the attitude error a^s as small as possible, all availabe information 
shall be used to correct the IMU measurement and to come with u)l'^ as close as 
possible to a;^**. If one would achieve u>l^ — u/f' in practice, the attitude of the 
body-frame w.r.t. the inertial-frame could be reconstracted exactly using the IMU 
measurements (despite errors due to numerical integration etc.). To come with a;** 
as close as possible to ljI^, it shall be 

u:f =. %u (c^T - ^t^ - hu) ■ (19) 



The term Tf,„ I a;™ — u:^ — b^ J is the best available estimate for the seeked an- 
gular rate ljI^, namely ujI^. Hence, a;^* = cJf'. So one can write 

ul' - a;^ - Tbu {i^T - '^'" - b„ - w„) - T^u [c^T - '^'" - k) (20) 
and 

ul' + a;^ = Ttu i^T - '^t" - b„ - w„) + T^u {^T - '^t" - k) ■ (21) 
With 



^;^ = (I-r - (1.^ - b„ (22) 



In this paper, IMU is equivalent to gyro unit. 
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and the general definition of the error Sx of an arbitrary variable x, namely 5x 
X — X, for Eq. (l20t one can write 



.ib 



(5u;°" - b„ + ^b„ - w„ 



And considering small errors (linear approximation) it follows that 

Ljf' - iof = T,„ Sut" + Tb„ <5b„ - <5T Tbu^l' - tfe.w^. 
Furthermore, using Eq. (l2T1 l. it can be written 

{ul" + a;r) X ab, = [(I - <5T) t,„ (cl;™ - a;^ - b„ - w„) 



(23) 



(24) 



X a;,,. 



+ i-bu^u 

And considering small errors (linear approximation) it follows that 

(u;f +0 X ab, - 2tb„u>;'' x ab,. 
So, finally Eq. dTSl ) becomes 



(25) 



(26) 



afcs 



Tbtio;^ 



abs 



TbuShu + Ti„,,5ui 



bu 



Tf„,a;„ 



(50-Tb„w„ (27) 



where 64> is the misalignment error and Slj^ its rate of change. Clearly, the equal 
influence of (5b„ and Su^ on k^s can be recognized. Their separation must be 
realized with different dynamic models. If the underlying dynamic models are equal, 
a separation is not possible. However, there might be applications where the exact 
knowledge of (5b„ and Suj^'- is unimportant. There, the focus is on rejecting the sum 
of both from at,s no matter what the individual values are. 
The state vector of the considered estimator shall be 



afes 

6(t> 

6(j} 



which leads to the corresponding continuous-time system matrix 




I 




where F; 



(28) 





- (tb„cl> 


?) 


Tbu 


[Tbu^l' 


F = 


























5ci>)/d 




(6<l>). 







^<A0 



(29) 
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The star tracker measurement q;,, and the attitude estimate q^i are used to for- 
mulate the filter innovation as quaternion 

(5q = qb, ®q;/. (30) 

It is converted into Gibbs vector formulation, a, for usage in the estimator. Hence, 
the measurement matrix of the estimator is 

H=[I000]. (31) 



3 Observability 

3.1 Theoretical Background 

This section goes beyond just presenting the well known equation for the observ- 
ability matrix. It rather focuses on its interpretation to draw conclusions on the ob- 
servability of each system state. 

An observer or estimator (e.g. Kalman filter) is often used to determine system 
states that cannot or only very difficult be determined. Such states could be the 
time-variant misalignment between star tracker and gyro unit. The question rises 
if all states can be determined/distinguished using the available sensor and system 
information. This leads to the question of observability which was initially defined 
by Kalman |41. 

Consider a system defined on some time interval T. An initial state xq = x(io) 
with to G T is said to be observable if it is possible to determine xo using the mea- 
surements z over a finite time interval ig < i < i/, t e T |l7,|. In other words, if the 
system is observable, the state xo can be reconstructed using the measurements z. 

In the following, it shall be distinguished between complete and non-complete 
observability. In the former case, each single state of the state vector can be deter- 
mined and, hence, distinguished from the other states. In the later, this is true only 
for some of the sates. The other states can only be determined in form of linear 
combinations of each other or not at all. 

Consider the linear time in-variant system of n states with state vector x(t) and 
measurement z(i) 

i(t) = Fx(i) (32) 

z(t) = Hx(t) (33) 

and state solution 

x(i) = e^ (*"*»' Xq. (34) 

The available information to evaluate xo can be summarized to 
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zit) 
i{t) 
i{t) 



z(t) 



(n-l 







"H 
HF 




= 


HF^ 


) 




HF' \ 



e^ (*-*«) xo 



(35) 



O 

O x(t). 



e^ (*-*«) xo 



(36) 
(37) 



Essential for the determination/observation of xo (and x(t)) is O, known as ob- 
servability matrix. The system is completely observable if and only if Rg(0) = n. 
Then, Eq. ( |36] | can be solved for xo (and Eq. dJTJ ) for x(t)). 



3.2 Application to Star Tracker I Gyro Fusion 
3.2.1 Observability Matrix 

With the simplified expressions 



T 

M 



Tbu 

u 

Fv^ 



the system matrix in Eq. ( [29] l becomes 



Tw) 


K T 


(Tu^y 


' T 























I 








M 






(41) 



And assuming small differences between b- and u-frame, hence, T w I, it follows 



(38) 
(39) 
(40) 



a;>< 


I 


w^ 


I 























I 








M 






(42) 



With the measurement matrix, Eq. (l3T1 l. one can write 
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HF = 


-LJ"" 


I 


a;x 


I ] 


HF2 = 


{u:-f 


-UJ"" 


-(u;X)Vm 


] 


HF^ = 


-(u;xf 


{u^xf 


{u-f 


M ] 


HF^ = 


(u^-f 


-(u;xf 


-(u;X)Vm2 


] 


HF^ = 


-iu;xf 


(u;x)^ 


(u;xf 


M2 ] 



(43) 



From now on, the observability analysis shall focus on spacecraft with angular rate 

(44) 



which is a good approximation for e.g. Earth observation satellites despite mis- 
sion/payload related steering. In this case, lo is equal to the orbital rate and constant. 
Hence, in addition to H, also F and therefore the system is time in-variant. 
So with 

'Ml 

M= A/2 (45) 

M-i 

it can be written 
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HF 

HF^ 

HF^ 

HF^ 

HF'^ 
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— tJ 
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— LO^ 
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LJ^ +Mx 



































Ma 

















-uo' 


LO 














Lj"^ + Ma 














u' 


-Lo'' 














-Lo'' 


Ml 



































Ma 


-U)^ 














2 


lJ' 











Ah 
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u,' 


-lo" + M't 



































M'i 

















u^ 


-L0-' 














-uo" + Mi 














^u^ 


LO^ 














co' 


m'I 



































M^ 


J^ 














u,^ 


5 











Mi 



(46) 

This is the upper part of the observability matrix. The parts HF for fc > 5 were 
left out. Because for the cases considered in sec. l3.2.2l thev do not gain additional 
information. 
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3.2.2 Interpretation of Observability Matrix 

The first 3 scalar lines of the observability matrix in Eq. (l46l l show that ats is always 
observable. This result is expected since a^s is directly provided as measurement to 
the filter (see Eq. dSOll). Writing Eq. ( l46t line by line gives 

[HF]^ - C21 ^ 6b^ + uj(t)z + (t)x 

[HF]2 - C22 = 5by + (fiy 

[HF]3 - C23 == (56,, - uj(t>x + (t>z 

[HF^] ^ - C31 = -uj 5b, + [u-^ + Ah)cp, 

[HF2J2-C32- + M2cl)y 

[HF^] 3 - C33 - c. <56, + (^2 _^ ^,^g)^^ 

[HF^] ^ - C41 == -tj2 ^5^ _ ^30, + A'h^^ 

[HF3]2-C42= M2(/)j, 

[HF^] 3 - C43 = -tc;2 ^6, + cu3(/), + A'h^, 

[HF^] ^ - C51 = ^3 ^5^ + (_^4 ^ ^2)^^ 

[HF4]2-C52= M^(j)y 

[HF4J3 - C53 = ~uj^ 6bx + {~cj^ + Aim, 

[HF^] ^ - C61 = cc;4 ^6, + Lu^(t), + mUx 

[HF5]2-C62= M^^j^y 

[HF5]3-C63= ^4^6.- Cc>5</,, + M|</., 

where Cij is a known value. It is known since a^s is always observable and, hence, 
computed from the first matrix line (H-line) in Eq. ( l46l ). The c^ correspond to the 
first matrix column in Eq. ( l46t . Using these equations, now different cases shall be 
distinguish to analyze the observability of the estimator states. 

Case 1 : Time-constant STR/IMU misalignment 

The time-constant misalignment leads to M = 0. Cancelling the last vector column 
in Eq. (l46t results in 

• Sby is fully observable 

• Sbx and (pz are only in combination observable 

• Sbz and (f)^ are only in combination observable 

• 0J, is not observable 

To determine the time-constant misalignment to extract its influence on the attitude 
estimate, calibration maneuvers are necessary. Such maneuvers must ensure the ob- 
servability of the misalignment states. 

Case 2: Harmonically oscillating STR/IMU misalignment with orbit frequency 

This case is close to Earth observation spacecraft with approximately equal sun and 
eclipse duration. One can write M = — cj^ I where ui is the orbital rate. Hence, 
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6bx, Shy and 5hz are fully observable 

(j)y is fully observable 

4)y is fully observable 

<t)z and (t>x are only in combination observable 

4)x and 4)z are only in combination observable 

The filter can not estimate each individual time-variant misalignment state. How- 
ever, in [gl it was shown that the influence of the time-variant misalignment could 
almost completely extracted from the attitude estimates. So, if the focus is on precise 
attitude estimates and not on precise determination of the time-variant misalignment 
states, this seams to be a valid approach. 

Case 3: Harmonically oscillating STR/IMU misalignment with doubled orbit 
frequency 

This case is more of theoretical nature to study the observability of the system states. 
One can write M = —Alo^ I. Hence, 

• 5hx, Sby and Sbz are fully observable 

• (px, 4>y and (pz are fully observable 

• (px, (py and (pz are fully observable 

4 Conclusion 

Deterministic time-variant misalignment between star tracker and gyro unit is 
mainly caused by temperature variation during orbit (e.g. sun/eclipse phases). It 
can significantly degrade the achievable attitude estimation performance when not 
considered by state augmentation within the sensor data fusion filter. The system 
dynamics equations required for state augmentation were derived. Considering typ- 
ical nadir-oriented Earth observation missions, the observability analyses showed 
that often the misalignment states can only be observed as linear combination with 
other filter states. Nevertheless, the proposed state augmentation can significantly 
decrease the influence of misalignment on the attitude estimation performance as 
shown in imiini. 
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Performance Comparison of Maneuver 
Detection Algorithms 

Sebastian Bayerl, Georg Herbold, and Lorenzo Pettazzi 



Abstract. This paper compares several algoritlims for target maneuver detection. 
The algorithms are based on input estimation coupled with Kalman filter or IMM 
filtering. The performance of each method is analyzed computing a Pareto frontier, 
which is based on different quality criteria such as root mean square (rms) of state 
estimation error and time of maneuver detection. It is shown that computation of 
such Pareto frontiers allows to compare the performance of the different detectors 
in a systematic way. 



1 Introduction 

In target tracking applications a maneuver detector can be used to upgrade an exist- 
ing model of a tracking filter. For example a filter modeling a constant velocity is 
working in a reliable manner, if a target is moving without any maneuver according 
to the filter assumptions. But, if the target motion differs to a constant velocity, the 
filter is estimating a false target state with a biased estimation error. The failure in 
modeling can be caused by any kind of maneuver. Thus a maneuver detection is 
used to give a corrected estimation of the target state, if the maneuver is according 
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to the maneuver detector model. Additionally a statement can be made whether the 
target is moving with a constant motion or performing a maneuver. This paper will 
focus on the maneuver detection and state estimation performance comparison for 
five maneuvering detection algorithms: 

• Open-loop input estimation with ;^^-based detector 

• Open-loop input estimation with Gaussian significance based detector 

• Closed-loop input estimation detector 

• IMM detector without interaction 

• IMM detector with interaction 

The remainder of this paper is organized as follows. In section 2 the problem of 
Maneuvering Target Tracking is defined. Section 3 presents several algorithms to 
solve this problem. In section 4 these algorithms are compared. Conclusions are 
presented in section 5. 

2 Problem Formulation 

A linear system is used to describe the motion of a target: 

x{k+l) = 0{k)- x{k) + G{k) ■ u{k) + q{k) (1) 

y{k)=H{k)-x{k)+n{k) (2) 

x(^) is the state vector of the target, y{k) is the measurement and u(^) is an unknown 
input, which is modeling a maneuver. The process noise q(A:) and the measurement 
noise n{k) are assumed to be uncorrected zero mean, white noise with known co- 
variance matrices Q{k) and R{k). H{k) is the measurement matrix. 

Maneuver detection consists of two different tasks: 

• On the one hand the existence of a maneuver has to be detected. Maneuver de- 
tection can be formulated as testing between two hypotheses: 

- Hypothesis Hi : The target is in non-maneuverig mode 1 (u(A:) = 0). 

- Hypothesis H2: The target is in maneuverig mode 2 (u(A:) 7^ 0). 

• On the other hand the estimated state vector x has to be corrected through the 
measurement vector y{k) . In order to obtain an accurate estimation, the maneuver 
u(^) has to be estimated and used to correct the state estimation. 



3 Detection Algorithms 

The basic building block of the maneuver detection algorithms presented in this 
paper is a Kalman filter with a constant velocity model (CV filter). This Kalman 
filter is able to compute an estimation x of the true target state x with associated 
covariance P. The transition matrix f& in (|T]| models a constant velocity without any 
maneuver. 
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x{k+l\k) ^ 0{k)-x{k\k) (3) 

P{k+l\k) = 0{k)- P{k\k) ■d'ikf + Q{k) (4) 

yik)=y{k)-H{k)-x{k+l\k) (5) 

Sik) =H{k)-P{k+l\k)-H{kf+R{k) (6) 

K{k) =P{k+l\k)-H{kf-S{k)-^ (7) 

x{k+l\k+l) =x{k+l\k)+K{k)-\{k) (8) 

P{k+l\k+l) = [I-K{k)-H{k)]-P{k+l\k) (9) 

The maneuver u can be detected and estimated by using the innovation v and its 
covariance matrix S. 

A maneuver detection algorithm can be build with two different approaches. The 
first way is to handle a maneuver as an unknown input into a system with constant 
velocity. Therefore the CV filter is extended with an input estimation algorithm. 
The other way to build a maneuver detector is to use an IMM filter system including 
two filters with different motion models. One filter is modeling a non-maneuvering 
motion, the other one is modeling a maneuvering motion. The structure of the filter 
system allows to get information about the maneuvering mode of the target. 

3.1 Input Estimation 

3.1.1 Preliminaries 

In Q it is shown that the innovation v of a Kalman filter is a linear function of the 
unknown input u{k). This can be easily shown by comparing the residuals of two 
different filters: 

• A false filter with non-maneuvering model: 

x{k+l)^0{k)-x{k)+q{k) (10) 

• A correct, but hypothetic filter with the maneuver as input: 

x*{k+l) = 0{k)-x*{k) + G{k)-u{k) + q{k) (11) 

With use of the residuals of the false filter, the unknown maneuver of the hypothetic 
filter can be estimated. The innovations v* of the correct, hypothetical filter are zero 
mean, white noise with covariance S*. In comparison with the hypothetic innova- 
tions the real innovations v are biased by the maneuver. Hence the real innovations 
can be written as a function of v* , i.e. 

v{k)==\*{k)+A{k)-u{k) (12) 

It is then possible to conclude that the real innovations are a linear measurement of 
the maneuver. A {k) is a measurement matrix defined with A (A:) = H{k) ■ G{k). 
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3.1.2 Open-Loop Structure 

Maneuver estimation 

To estimate the unknown maneuver u and its covariance matrix Pi, a derivation of 
the recursive least square method according to [2j is used. 

Pu{k)= [P:'\k~\)+A'{k)-S-\k)-A'{k)Y' (13) 

vi{k)^vi{k-\)+A\i{k) (14) 

with Avi{k)=Pu{k)-A^{k)-S-\k)-[\{k)-A{k)-vi{k-\)] (15) 

This estimation differs to the standard recursive least square method. The estima- 
tion of the unknown input u with the standard least square method would lead to 
a smaller covariance matrix with each new time step until new measurements have 
almost no influence to the estimation. In order to mitigate such problem the old 
covariance matrix Pu{k— 1) will be rescaled with a factor ys [0, 1]. 

P:{k-l)^yP.{k-l) (16) 

Maneuver detection 

In order to detect if the target is in maneuvering mode, tests for the statistical sig- 
nificance of the maneuver can be introduced, such as the x^ and the Gauss test. 

(H). 

For the x'^ t^st a variable e(^) ~ u(A:)-^ • P„{k)^^ ■ u{k) is calculated. Under as- 
sumption of Hi, e{k) is x^ distributed. This leads to a simple test. 

e(A:) > ;t («, a) => H2, target is in maneuvering mode (17) 

where (1 — a) is the confidence level of the test. This means Hi will be rejected with 
confidence (1 — a), if e(^) exceeds the corresponding threshold, n is the degree of 
freedom of the estimated maneuver. 

Similar to the x^ test is the Gauss test. According to the Gauss test a maneuver 
will be declared, if one single component u; of the estimated maneuver is statistical 
significant. A is determined from the standard Gaussian distribution. 

' >A => 7/9, target is in maneuvering mode (18) 



JPui,i{k)^ 

State correction 

If the estimated maneuver has been estimated and it is statistical significant, the state 
and covariance must be corrected. In the open-loop structure only the output of the 
system is being corrected so that the corrected state estimation is not fed back to 
the filter. In order to characterize the effect of the maneuver onto the state, a time 
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window is being built continuously. The matrix ^ describes the windowed relation 
between the target state and the maneuver. It is calculated as follows: 



^{k) = [I-K{k-l)-H{k-l)]-0{k-l)-'F{k) + G{k) 



(19) 



According to [21 the estimated state and its covariance can be corrected in the fol- 
lowing way. 

Xcorrik\k) = x{k\k) + Ax{k) 

= x{k\k) + [I-K{k) ■ H{k)] ■ »P(yfe) • u{k) (20) 

Pcorrik\k)^P{k\k)+AP{k) 

= P{k\k) + [I- K{k) ■ H{k)\ ■ ^(yfe) • P„(yfe) • 'F^(yfe) • [/ - K{k) ■ H{k)f 

(21) 
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Fig. 1 Maneuver detection and state correction with open-loop structure 



3.1.3 Closed-Loop Structure 

In the open-loop structure the detector does not share the information about the esti- 
mated maneuver with the filter. The corrected state and covariance is only available 
at the output. According to |@1 the optimal estimation is possible, if a closed-loop 
structure is used, where the feedback of the filter is corrected. 

Through this closed-loop structure it is not possible to make a windowed estimate 
of the maneuver, because the filter state is being corrected each time step. Instead 
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Fig. 2 Maneuver detection and state correction with closed-loop structure 
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of a time window each single measurement has to be used separately to estimate the 
maneuver. 

Maneuver estimation 

At the beginning of this section it is shown, that the residuals are a linear measure- 
ment of the maneuver. Thus the maneuver and its covariance can be estimated via 
least square methods according to IS . 

u{k) = [A'^{k)-S-\k)-A{k)y^ -Aikf -Siky^-yik) (22) 

Puik)^ [A^ik)-S-\k)-A{k)y' (23) 

The estimation of the maneuver, out of the innovation without windowing, means to 
loose information about the composition of the innovations e.g. it is not possible to 
extract the acceleration from the innovation noise. This results in an estimation of 
a maneuver caused by noise. In order to reduce this effect several algorithms were 
tested. The best results were reached by filtering the estimations with an filtering 
factor a e [0,1]. 

u{k) = {l-a)-u{k~l) + a-u{k) (24) 

Puik) = {\-af-Pu{k-\) + a^-P,{k) (25) 

Maneuver detection 

The maneuver is detected by an absolute test. This means one single component u; 
of the estimated acceleration has to exceed a threshold. This threshold is defined in 
adaptive fashion through the process noise of the Kalman filter. Avfut^r is the veloc- 
ity noise of the process noise Q and dt is the elapsed time since the last detection. 

Ui > Ufhreshoid -^ //2> target is in maneuvering mode (26) 

■tU '^^' filter 

Wltn Uthreshold = — Jt — 



State correction 

If the maneuver is detected, the filtered state and state covariance can be corrected. 
The correcting term is calculated similar to the open-loop structure. 

(27) 
(28) 

with Ax{k) = [I- K{k) ■ H{k)] ■ G{k) ■ a{k) (29) 

{k)-[I-K{k)-H{k)f (30) 





±corr{k\k) -- 

PcoAm = 


= x{k\k) + Ax{k) 
= P{k\k)+Ap{k) 


Ax{k) = 
Ap{k) = 


--[i-K{k)-m)] 

--[I-K{k)-H{k)] 


■G{k)-u{k) 
■G{k)-P,{k)-G'^. 
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3.2 IMM Filtering 

The algorithms presented in section lT. 1 .2l and [3. 1 .3l are both based on a combination 
of a filter which is modeling a constant velocity, and a detector. In fT\ an algorithm is 
described which is using several tracking filters with different motion models. Hence 
the name interactive multiple model filtering (IMM filtering). The IMM algorithms 
used in this paper is using two different filter: 

• One filter with a model describing a non-maneuvering motion 

• One filter with a model describing a maneuvering motion 

The maneuvering filter model can describe several maneuvers. For the comparison 
in this paper, a maneuver with constant acceleration is modeled. There is a fixed 
initial switching probability Pj; , which describes the probability that the target state 
will switch from mode / to mode j in the next time step. Furthermore there are other 
conditional probabilities which are being updated by the innovations of the filters: 

• The switching probability jXi.j{k) describes the probability that the target has 
changed from mode / at time step (k-1) into mode j at the actual time step (k). 

• The fixed switching probability Pj^ describes the probability that the target will 
change from mode / at the actual time step (k) and to the mode j in the next time 
step (k+l). 

• The state probability jJLi{k) describes the probability that the target is in mode i at 
time (k). 

• The predicted state probability Ci{k) describes the probability that the target will 
be in mode i at time (k+l). 

3.2.1 Probability Update 

As in the x^ test, statistical deviations x} of each filter are calculated to update the 
probabilities of each filter i: 

X}{k)=yi'{k)-sr\k)-yi{k) (31) 

expf— 5■7?f^')) 

The probability density can be then obtained with AAk) = , '. , ' . 

Where N is the degree of freedom of the innovations. Finally the probabilities 
can be updated as follows: 

2 2 

with C{k) = '^Aj{k)-Cj{k) and Cjik) ^"^Pj^y ^i{k- 1) (33) 

.7=1 i=l 
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3.2.2 IMM Interaction 

It is possible to mix the states and covariances in the feedback of all filters. This 
interaction leads to an exchange of information between the filters. 

2 
Xj-,nixik-l\k- 1) ^ j^H,j{k- l)-X,{k-l\k- 1) (34) 

1=1 

2 

Pj-,nixik-l\k- 1) = ^^uik- 1) • [Piik-l\k- l)+DPij{k-l)] (35) 
1=1 



with DPij (A: - 1 ) = Dx;y {k- I) -Dxijik- I) (36) 

lyxijik^ 1) = x,(^- 1|^- l)-xj^„uxik- l\k- 1) (37) 

3.2.3 Maneuver Detection 

The state probabilities /i,- of each filter ; contain information about the target maneu- 
vering mode. If the state probability of the maneuvering filter exceeds the probabil- 
ity of the non-maneuvering filter, then the hypothesis H2 is correct. This leads to a 
simple test: 

l^mimeuveringfiiter > 50% => H2, target is in maneuvering mode (38) 

3.2.4 Combination of the System Output States and Covariances 

Instead of correcting the state and covariance of the non-maneuvering filter, the 
filtered data will be combined. Thus, a weighted sum of the states and covariances 
is being built. Furthermore, it is not necessary to estimate the maneuver because 
it is already estimated in the maneuvering filter. These combined values are not 
significant for the IMM algorithm itself, but they are used for a possible user at the 
system output. 

2 

xik\k)^'^lii{k)-Xi{k\k) (39) 

7=1 

Pik\k) = ;X Hi{k) ■ {Pi{k\k) + [xi{k\k) - x{k\k)] ■ [xi{k\k) - xik\k)f} (40) 
1=1 



4 Comparison of the Algorithms 
4.1 Criteria of Quality 

Three criteria of quality are defined to compare the different algorithms. 
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The first criterion is the delay time. It is defined as the arithmetic mean of the 
time the detector needs to detect the maneuver onset and the time the detector 
needs to detect the maneuver termination. 

The second criterion is the number of false detections. A false detection is a false 
statement of the detector about the maneuver. For example, a detector detects no 
maneuver during the period the target is maneuvering. 

The last criterion is the estimation error which is a measurement of the noise 
affecting the maneuver estimation and state correction. In order to obtain the 
estimation error a time difference between the true maneuver and the mean es- 
timation of several Monte Carlo simulations is calculated via cross-correlation. 
This time difference is used to shift the maneuver estimation of one single Monte 
Carlo simulation towards the true maneuver. The estimation error is the differ- 
ence between the shifted estimation and the truth. The root mean square of this 
difference over time is built to get the estimation error. Important is that areas of 
At around the maneuver start and end will not be considered. These manipula- 
tions are needed in order to avoid the influence of the estimation delay into the 
calculation of the estimation error. Finally, this time averaged error is addition- 
ally averaged over all Monte Carlo simulations. 




reference 
estimation 
shifted esttmatior 
estimation error 



Fig. 3 Calcluation of the error in maneuver estimation 



4.2 Pareto Frontier 



Comparing two or more algorithms is difficult because the algorithms differ in more 
than one criterion of quality. Thus, a Pareto frontier according to |l5l| is used to com- 
pare the detection algorithms in a plane with estimation error and the detection delay 
as axis. The Pareto frontier is the connection line between all dominant points of one 
algorithm. The attribute dominant is declared, if there is no other point which has 
a lower detection delay and a smaller estimation error. This means that one point 
on the Pareto frontier describes the best estimation error at a fixed delay time and 
vice versa for a special scenario. To get one point in the plane a simulation with 
fixed input parameters [pi, . . . ,/?„] will be calculated. These input parameters have 
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influence on the behavior of the detection algorithms. After the simulation is done, 
all criteria of quality are calculated and used to localize a point in the Pareto plane. 
This point is representing the behavior of the algorithm with the input parameters 
[pi,..., Pn]. To approximate the Pareto frontier each input parameter /?, will be sam- 
pled with [p'P'" ,..., pf"'^] . In this paper the input parameters for the input estimation 
based detectors are the process noise Qcv and the filtering factors a I y. For approx- 
imating the Pareto frontier of the IMM algorithm the two process noise matrices Qt 
of each filter / and the initial switching probability Pt are varied. 



4.3 Comparison of the Pareto Frontiers 

A scenario has to be defined to compare the algorithms. In the simulated scenario a 
target is moving with constant velocity v until a maneuver with constant turn begins. 
The acceleration of the target during the maneuver is defined with a{k) — (Ox \{k). 
The simulated trajectory has duration of 100 seconds. The maneuver starts at 25 
seconds and ends at 75 seconds. The radar measures in radius r, azimuth (p and 
elevation 0. Radar noise is added correspondingly. The following parameters are 
used to simulate the trajectory: 



radar position po 

error of measurement G, = 

initial target position po = 
initial target velocity vo = 
turn rate CO = 0-0-0.1 



Om Om Om 

20 m, 



a^ = 3.4mrad, Og 

T 

10000m Om 1000m 



1 .7 mrad 



250^ 

T 



021 0'. 



Figure|4]shows the Pareto frontier of this constant turn motion with a maximum part 
of 25 % false detections. 

• Maneuver detection by IMM-filtering has best performance. It has the best de- 
tection delay at a fixed maneuver estimation error. The interaction of the filters 
does not significantly improve the performance of the detector. 

• Maneuver detection with input estimation has a inferior quality to the IMM de- 
tection. With open-loop structure the x^ and Gauss test have almost the same 
results. 

• The maneuver detection with closed- loop structure shows nearly the same perfor- 
mance as the open-loop structure. The advantage of the information interchange 
between filter and detector is nullified by the fact that the estimation has to be 
filtered strongly in order to avoid a feed through of the measurement noise to the 
maneuver estimation. The closed-loop estimation does not achieve a low noise 
estimation, because the strong filtering, which is needed, would cause an enor- 
mous detection delay. 
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IMM ( without interaction ) 

IMM ( with interaction ) 
—~- Input Estimation ( open ioop, chi-square test ) 
— Input Estimation ( open ioop, Gauss test ) 
Input Estimation ( closed ioop, absolute test ) 




estimation error {acceleration) (m/s ) 
Fig. 4 Pareto frontier for a constant turn motion 

4.4 Comparison of the Computing Time 

In this section the maneuver detection algorithms are analyzed with respect to the 
computing time. This is also a critical quality criterion for real applications. The 
computing time is calculated as follows. With each maneuver detection step the 
calculation time is measured and saved. After the simulation all these values are 
taken and averaged in time. This average is additionally averaged above several 
Monte Carlo runs. The given data in figure |5] are scaled to the longest computing 
time in order to avoid absolute values. 

• The IMM algorithms need the most calculating time. The reason lies in the two 
parallel running Kalman filters and the complexity of the algorithm structure. 
The IMM algorithm without interaction can be processed faster than the IMM 
algorithm with interaction. 

• The calculation time of open-loop input estimation with x^ test is a bit lower 
than those of the version with Gauss test. 

• The closed-loop input estimation has the lowest computing time. 



Closed-loop input estimation 

Open-loop input estimation with Chi-Square test 

Open-loop input estimation with Gauss test 

IMM detection without interaction 

IMM detection with interaction 




Fig. 5 Relative computing time of the maneuver detection algorithms 
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5 Conclusion 

Several maneuver detectors are developed to detect a maneuver and to estimate the 
target state. With the use of one Kalman filter with non-maneuvering motion model 
the state can be estimated and corrected via least square methods. A state estimation 
and maneuver detection is also possible by using two Kalman filters with different 
motion models in an IMM algorithm. Also a special scenario was defined, in which 
the maneuver detectors are compared. Note that the performance of the maneuver 
detectors has been assessed against a maneuvering scenario that does not match 
with the one assumed in the input estimation and the IMM algorithm. It is shown, 
that different maneuver detection algorithms with several criteria of quality can be 
compared with the use of a Pareto frontier in a systematic way. The results of this 
comparison show, that IMM filtering has on the one hand best maneuver detection 
and the best estimation behavior but on the other hand the highest computing time. 
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Spacecraft Attitude Estimation and Gyro 
Calibration via Stochastic H^ Filtering 

Daniel Choukroun, Lotan Cooper, and Nadav Berman 



Abstract. A filter for estimating spacecraft attitude quaternion and gyro drift from 
vector measurements in the presence of white noises in the gyro error, in the drift 
dynamics, and in the line-of-sight measurement error is developed. The variance pa- 
rameters of the white noises are unknown, and are modeled as non-anticipative sec- 
ond order stochastic processes. The approach taken in this work consists in estimat- 
ing the attitude quaternion and the gyro drift while attenuating the transmission from 
the unknown variances to the estimation error. The resulting Hoo filter involves the 
solution of a set of (differential) linear matrix inequalities. In the case of gyro white 
noises, extensive Monte-Carlo simulations were run showing that the proposed filter 
performs well from the standpoint of attitude estimation per se, in a wide range of 
gyro noise and line-of-sight noise intensities. The guaranteed disturbance attenua- 
tion level seems to be slightly dependent on the noises intensities. The actual level of 
disturbance attenuation is improving when the noises levels increase and admits as 
worst scenario the case of(ideal) noise-free sensors, as expected from the analysis. 
When compared with a matched quaternion Kalman filter, the Hq^ filter produces 
higher Monte-Carlo standard deviations of the estimation error, but lower Monte- 
Carlo means. The higher the level of noises are, the less obvious the advantage of the 
Kalman filter is. When estimating the quaternion only, and as opposed to standard 
quaternion Kalman filters, the Hao filter's gains can be computed independently 
from the quaternion estimates, which makes it insensitive to estimation errors. 
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This favorable feature is further emphasized when comparing its performances with 
those of unmatched Kalman filters. When provided with too high or too low noise 
covariances, the Kalman filter is outperformed by the Hoo filter, which delivers es- 
sentially identical levels of errors within a wide range of noise intensities. 

1 Introduction 

The attitude quaternion ||T|, q, is a very popular spacecraft attitude parametrization 
and the mathematical modeling and filtering have been ongoing topics of research 
for more than four decades [:2|. Classical techniques for quaternion stochastic esti- 
mation belong to the realm of Kalman filtering, i.e., optimal filtering, and numer- 
ous successful quaternion estimators have been developed along that approach (e.g. 
in, H). An inherent drawback to the optimal filtering approach consists of being 
sensitive to the model parameters' inaccuracies, in particular, the estimator's per- 
formances rely on the adequate knowledge of the measurement and process noises 
variances. Although adaptive noise estimation might provide satisfactory perfor- 
mances for some cases ||5l, the designer may be willing to tackle the parameters 
uncertainty pitfall by a less sensitive approach: rather than trying to estimate the 
uncertainty, the filter will attenuate their effect on the estimation error down to an 
arbitrary transmission level. In that case, the system satisfies the L2-gwn. property 
between the perturbation and the estimation error. That approach was applied in |[6l, 
where an Hao spacecraft quaternion and gyro bias estimator was developed. The 
point of view of that work was deterministic, in the sense that the measurement and 
process noises were modeled as deterministic functions. 

In this work, building on a previous work |7|, a stochastic Hq^ quaternion fil- 
tering problem is proposed. We consider the case where a single continuous-time 
noisy vector measurement is being acquired and a triad of body-mounted gyros are 
providing a measurement of the spacecraft angular velocity with drift errors and 
white noise. We assume that the drift behaves like a non-zero mean random walk 
and that the variances of the line-of-sight measurements, of the gyro errors, and of 
the drift dynamics are unknown. The proposed filter is designed such as to estimate 
the quaternion and the gyro drift while attenuating these unknown perturbations. 
Motivated by recent works on stochastic Hoo filtering and control for nonlinear sys- 
tems 191, ifTOll . we follow an approach based on the dissipativity theory. 

As a first step, an Hoo filter is developed where the variance of the gyro noises 
only is modeled as a perturbation. The measurement noise level is known and the 
drifts are assumed to be zero. The filtering problem solution hinges on solving a 
(differential) linear matrix inequality (DLMI) in order to compute the filter's time- 
varying gain. The DLMI provides a sufficient condition for the sought L2-gwn. prop- 
erty. The development avoids linearization, and the structure of the quaternion state- 
space equations is exploited such that the resulted DLMI does not depend on the 
estimated process. As a consequence, computations can be performed off-line for a 
priori known histories of the line-of-sight measurements and of the angular velocity. 
Further, another Hoo filter is developed along the same approach, but assuming that 
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both the gyro white noise variance and the attitude sensing noise variance are un- 
known and modeled as disturbances. The resulting estimator shows similar features 
to the first one. Extensive Monte-Crlo simulation were run in order to check the 
Hoo filter's performances, as a quaternioin estimator, and to compare the perfor- 
mances of this type of estimator with another type of quaternion Kalman filter. The 
Kalman filter is a variant of the classical Multiplicative EKF for quaternion estima- 
tion [31 . The case of quaternion and gyro drift Hoo estimation was also addressed 
analytically. 

Section [2] presents the mathematical formulation of the problem. Section |3] in- 
cludes the development of the quaternion Hq^ estimators. Section |4] provides the 
basis for the development of a quaternion-drift iJoo estimator. Section [5] presents 
the results of the Monte-Carlo simulations. Conclusions are proposed in the last 
section. 



2 Statement of the Problem 

Consider the following stochastic dynamical system in Ito form: 

dq,= ir2(a;, -cjq,dt- is(qj(7(049,; q(0) =' qo; ie[0,r](l) 
cfc, = cr(t)diyj c(0)=Co (2) 

dy, =if,q,dt- iH(qJa,(t)dr,, (3) 

where q^ denotes the attitude quaternion, fit is the following matrix function of the 
measured angular velocity Wt, 



nt 



J^ 



(4) 



where w^ , which is acquired by a triad of body-mounted gyroscopes, is corrupted 
by an additive drift, c^, and by an additive standard Brownian motion, /3f, with 
infinitesimal independent increments c^j such that £'{c^jC^ } = ^a c^i- ^_^) de- 
notes the variance parameter of the gyro output noise /3 . The gyro drift is modeled 
as a random walk process with mean Cq and variance parameter cr(i). In Eq. Q, 
v^ denotes a standard Brownian motion that is independent from /3 . The system of 
equations ([T]l, (|2ll is a straightforward extension of the quaternion stochastic differ- 
ential equation (SDE) developed in 1 8] to the case of drifts in the gyro output error. 
Equation (O is the continuous-time equivalent of the quaternion measurement equa- 
tion in Q, where the measurement value is identically zero. Hence, 

dy, = (5) 

and the measurement matrix Ht is constructed from vector measurements. Let bt 
and Vt denote the projections of a measured line-of-sight (LOS) in the spacecraft 
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body frame axes and in a reference frame, respectively, then Ht is computed as 
follows 



d = 

H = 



2(b+r) 
i(b-r) 



[sx] d 
-d^ 



(6) 
(7) 
(8) 



The matrix, ^ , which appears both in the process and measurement multiplicative 
noises, is the following linear matrix function of the quaternion q = [eP" qY': 



^(q) 



ql^ - [ex] 



(9) 



The measurement noise is modeled as a standard Brownian motion, n , with vari- 
ance parameter cr {t). 

The filtering problem consists in estimating the quaternion, q^ , and the gyro drift, 
Cj , from the LOS measurements in the presence of unknown intensities in the sys- 
tem noises, q{t), cj [t), and cr [t). The filtering problem is formulated as a stochastic 
disturbance attenuation problem via the iJoo approach in the following way. Assum- 
ing that q{t), q{t), and cr(t) are stochastic non-anticipative processes with finite 
second-order moments, we consider the following estimator: 



<^. 



1 



^{^t -cjq, (it + /v,(q^,cj(dy^ -^.^dt) 



<£t = Kc{%.c^) (dy^ - q, dt) 
q(0) = qo, c(0) = Cq 

Let q^ and c^ denote the additive quaternion and biases estimation error, i.e., 



q* =q, 



q* 



(10) 

(11) 
(12) 



(13) 
(14) 



Given a scalar 7 > 0, we seek for a gain process K{ci^ ) such that the following Ha 
criterion is satisfied: 



E{ 



|qJP + ||ejP)dO<7'i^{||qo|P + ||eo| 



Ivjpdt} (15) 



under the constraints ([Til-®; and where v^ denotes the augmented process of ad- 
missible disturbance functions, i.e., V4 ~ {'?(0i '^(Oi '^(^)}- Whenever Eq. (fT5l l 
is true, it is said that the L^-gain property is satisfied from {qo,vt} to q^, for 
< t < T. 
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3 Quaternion Hoc Estimation 

3.1 Attenuation of Process Noise with Unknown Intensity q 

As in Q, we will first consider the simpler case where there are no biases and the 
intensity parameter cr is known. 

Augmented stochastic process 

Following standard techniques [9| , we define an augmented process as follows: 



cC 



(16) 



It is straightforward to develop the following SDE for the augmented process <f : 









dt + J2 



04X1 

1 w 
- 2 "C». 



q(t)dP. 



OaxS 



(17) 



where Sci ,i = 1,2,3, denote the columns of the matrix S, and the scalar processes 
/?., i = 1,2, 3, are the components of the vector Brownian motion (3 . Notice that 
H is a function of the augmented process {q^ , q^ } since it is a function of the 
quaternion q. Equation dTTI ) can be re-written in the following compact form: 



df^=F-ct^dt + Y, sU<fMit)dP. + G«)dr?* (18) 

1=1 

where F" , g2(<^) and G{(f) are effectively defined from Eq. i ITT] ). 

Hamilton- Jacobi-Bellman inequality 

Following 191, the desired L2-gain property will be satisfied if and only if the aug- 
mented system dTSl l is dissipative with respect to the supply rate S{q{t),(f) = 
7^[q'(i)]^ — ||qj |p, for a given positive scalar 7. We, thus, seek for a non-negative 
scalar-valued function, V{(f , t), that satisfies the fundamental property ifTOll : 



E{V{ci,t)}<E{V{cf,,s) + j\j^q{, 
F(cfo,0)<7'||qoir 



'-)dr} V < s < t < T 

(19) 
(20) 
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for all cf and for all admissible q{t). When Eq. ( fT9] l is satisfied, the function V is 
called a storage function with respect to the supply rate S. A sufficient condition for 
Eq. (O is: 



E{dV{cf,t)}<E{j'\\q{t)\\' 



'-} VO < t <T 



(21) 



for all cf and for all admissible q{t), where dV is the Ito differential of the function 
V. Using Ito differentiation rule 1,11 J and dropping the expectation operator on both 
sides of Eq. (l2T1 l yields the following sufficient condition [Hamilton-Jacobi-Bellman 
(HJB) equation] for V{cf,t): 



dV_ 
dt 



dV 



1 2 \r.r.T ^^ 

- cr/ tr <^ GG^ r, r, T 



(22) 



+ W^t)± 



§2 






<l^[q{t)f-H£ 



for all < t < T, cf , and for all admissible q{t), where G = G{(f). Bringing all 
terms to the left-hand-side (LHS) of Eq. (l22l l yields 



dt d(f^ 2 " [ 



cf^Lcf 



(9cf(9cf 

3 o2 



(23) 



and where 



L 



9 IIg2 



'04 04 



d'v 



d(fd(f 



^g2 - 7 



q^{t) <0 



(24) 



For a solution to exist for all cf and for all admissible q{t), the coefficient multi- 
plying the arbitrary disturbance function, q{t), in Eq. ( |23] | must be negative, which 
yields the following condition: 



1 ^ 

oEg2 



d<fd<f' 



Tg2 - 7' < 



(25) 



for all (cf ,i). Given Eq. ([25]), any non-zero disturbance will only add a negative 
term to the LHS, increasing the system's dissipativity with respect to the chosen 
supply rate. The worst-case scenario will, thus, consists in a vanishing disturbance, 

q*{t),t.g.. 



q*it)^0 
Inserting Eq. ( l26b into Eq. ( |23] ) yields the following sufficient condition: 



(26) 
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for alio < i<Tandcf. 

Candidate storage function V 

A standard approach in order to circumvent the formidable task of solving the par- 
tial differential inequality for V [Eq. dZTl ll consists in guessing the solution in a 
parameterized form and developing sufficient conditions for its parameters. We use 
here the classical quadratic form for V, i.e., we assume 



Vicf,t)^cf^P^ct 



(28) 



with the further assumption that P^ is symmetric, positive, and block diagonal, i.e.. 



P 



Convexity condition with respect to q{t) 



P, Oj 
O4P, 



(29) 



The existence of a solution for the filtering problem is conditioned upon Eq. (|25] ). 
which is re- written here; 



1 ^ 



T d^V 



2 ^°^ acfScf^ 



§2 - 7' < 



Using Eqs. (l28t,(|29]). and the following known property of the matrix ^ 
without simplification via the quaternion unit-norm constraint, yields 



% 



^{trPJ,-Pj-j'l, 



q. <o 



(30) 



(31) 



(32) 



Since Eq. 



must be satisfied for all q^ , we deduce the following condition on 7: 

1 



{tvPJ,-Pj-Yh<0 



(33) 



Sufficient condition on the matrices K and P 



Using Eqs. 
identity: 



in Eq. dZTJ ). straightforward computations yield the following 
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[ 04 '^ + fJp,+p,fS 




(34) 



where F, is defined as follows: 



t t ^ 



(35) 



Relying on Eq. ( |3T1 ). and using the expression for G, as given in Eq. ( fTTI ). further 
algebraic manipulations yield the following identity: 



- ct/ tr <^ GG-' - — —^ 
2 " \ dcfd(f^ 



=q* It 



=[qfqn 

[/4 h] 



Xr[KjP^K^)h-KjPJ<^ 
.2 



q* 



tr(irfP,ifJ/4-irfP,if, 



(36) 



Using Eqs. ( l34l l and (l36l l. we are able to express the LHS of Eq. (IT7t as a quadratic 
form in cf , and thus to re-write Eq. ( l27l i as follows: 



jf + F.^F, + F.F, + -|- [trJV/, J4 - A/. J 



+ [trM,/4-M,J 
■ + F'fP, +p^F,+\- h-cMJi - hi} + h 



<0 



(37) 



for all (q^Qtii), where M^ ~ K^P^K^. Inequality (|37] ) yields the following dif- 
ferential matrix inequality 



_J. + F^Pj + Pj F + -|- [( trM)/4 - m\ -|- [( tri\/)/4 - m\ 

\- [{ trM)/4 - m\ ^ + P'^Pj + Ft F + ^ [ { lrM)/4 - mI + /i 



for all < i < T, where 



<0 

(38) 



F= -Qt- KtHt 



(39) 
(40) 



and we dropped the symbols ^ in order to emphasize that the gain matrix, Kt , will 
not be a function of the estimate. 
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3.2 Attenuation of Process Noises with Unknown Intensity q and 
of Measurement Noises with Unknown Intensity or 

In the present case, the intensities of both the gyro white noise, q, and of the attitude 
sensor's noise, cr , are assumed unknown and considered perturbations in the -ffoo 
estimation problem. The filtering problem is formulated, as previously, as a stochas- 
tic disturbance attenuation problem via the Hoo approach, where the intensities are 
modeled as random non-anticipative processes with finite second-order moments on 
[0, T]. For the sake of brevity, and thanks to the similarity between the current case 
and the previous one, the filter development details will be omitted. We consider the 
following estimator: 



q* (0) = qo 



^f2t- K,Ht 



q. dt (41) 



where K^ denotes /l'(qj. Let q^ denotes the additive estimation error, i.e., q^ = 
qj — q^ . Given a positive scalar, 7 > 0, we seek for a gain process, {K_^ , < i < T}, 
such that the following if 00 criterion is satisfied: 

E{f \\q,fdt}<j'E{\\cio\\'+f htW'dt} (42) 

Jo JQ 

where vt denotes the augmented process of admissible disturbance functions, i.e., 
^t = {o:{t)i <^(0}- Whenever Eq. ( |42] | is true, it is said that the L2-gain property 
is satisfied from {qo,q'(i), cr(t)} to q^, < i < T. Simply stated, the proposed 
estimator is designed such as to provide an attitude estimate, with a given level of 
attenuation from the initial error and the process and measurement noise levels to 
the estimation error. An attractive feature is that no a priori knowledge of the noise 
intensities is necessary and the attenuation performance is, thus, guaranteed for a 
very wide class of noise levels, which may be random and time-varying, provided 
that they have a second-order moment. 

Sufficient conditions on the matrices K, P, P 

The sought sufficient conditions on K, P^ , and P^ are as follows: 

^+F^P^+P^Ft<Q (43) 

^+F^P,+P,Ft+I,<Q (44) 

i(trF,4-Pj-72/, <0 (45) 

i(trAft4 -M0"7'^4 <0 (46) 
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where 

Ft=^nt- KtHt (47) 

Mt=KjP^Kt (48) 

For simplification purposes, it is assumed that the matrices P^ and P^ are identical, 
i.e., we seek for a single positive definite matrix, P^, that satisfies Eqs. (l43l l to (l46t . 
Extensive simulations showed that this assumption did not significantly impair the 
estimator's performances. Moreover, this allows dropping Eq. (l43l l. which is auto- 
matically satisfied if Eq. ( |44T ) is satisfied. The simplified sufficient conditions under 
the assumption of a single decision matrix variable P^ are 

(49) 

(50) 

(51) 

where 

(52) 

(53) 

Since the LHS of the above inequalities is independent from the quaternion estimate, 
the sought filter gain, Kt, will be independent from q^ . 

Sufficient condition inform of Linear Matrix Inequalities (LMI) 

Since the above inequalities are not linear with respect to P and K, some manipu- 
lations are required in order to bring them to an LMI structure. The bilinear depen- 
dence with respect to P and K is readily coped with via a standard parametrization 
approach. Let Y^ denote the following four-dimensional matrix: 

Y.^PJU (54) 

then, using Eq. (l54l l in Eq. ( l49l l yields 

dP 1 ~ ~ ~ ~ 

^ + - (n^P, + P,Qt) - {HjY^ + Y,Ht) + /, < (55) 

In order to circumvent the difficulty arising from the quadratic structure of Mt with 
respect to P^ and K, we seek for a symmetric positive definite matrix Wt, such that 

Mt-Wt^ Y^Pr'Y^ -Wt<0 (56) 



dp, 

dt 


■+F^ 


P,+P,Ft+I, 


<o 


V 


tr^. I. 


-PJ- 


i^h<o 


V 


trMt I, 


-Mt) 


-I'h 


<o 




Ft = 


w 


KtHt 






Mt = 


-'KjP.Kt 
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Notice that P ^ exists since P^ is assumed to be positive definite. Then, we use the 
following bounds on the LHS of Eq. (fSTI l: 

i ( trMt I, - Mt) ~l^h<{\ tvMt - 7') ^4 < ( J trW^t " 7') ^4 < (57) 

and replace Eq. (l49l l by the following sufficient condition on W: 

1 



trWt - 7^ < 



(58) 



where W, Y, and P satisfies Eq. 
written as the following LMI : 



which by the Schur complement can be 



-Wt -Y, 
-Y^ -P 



<0 



(59) 



Notice that the successive bounds in Eq. ( fSTJ l may yield higher feasible values for 
7, i.e., a worse level of guaranteed disturbance attenuation. 

Summary of the continuous-time H 00 filter 

Given qo, choose P(0) such that Eq. ^ is satisfied. Solve for P^ = P^ > £ M^, 
y; e W^, and Wt = W^ >Qe R'', the following set of (differential) LMIs: 



^ + \ [QjP, + P,Q,) - {HjY^ + Y,H,) + /, < 



~Wt -Y, 
-Y^ -P 



<0 



(trP,/,-Fj-72/, <0 
trWt - 7^ < 



(60) 

(61) 

(62) 
(63) 



For any pair of matrices (YJ , ^^ ), compute the gain Kt using 

Kt = P-^Y 

^ t t 

and compute the estimated quaternion via the estimator differential equation 



(64) 



1 



f^t - KtHt 



q* 



(65) 
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Remark 1: The estimator equation, (I65] ). is not designed to preserve the quaternion 
unit-norm property. For that purpose, a normaHzation stage of the estimate is per- 
formed along the estimation process, i.e., q^ (0) is continuously divided by its norm. 

Remark 2: Inspired by previous works llT2l . the discrete approximation of the dif- 
ferential LMI ( l6Ql l. is developed via a finite-difference formula, which is here: 

^'^^~^' + I {Ol+,Pk+i+Pk+ink+i) - (i^J+iFj+i + Ffc+ii/fe+i) + /, < 

(66) 

where At denotes the time increment, and k = 0,l,...,N~T/At. 

4 Quaternion and Gyro Drift H^o Estimation 

In this section, the problem of estimating the attitude quaternion and the gyro drift 
under the assumption of unknown intensities in the system's noises is addressed. 
The quaternion-drift system is governed by the following SDE: 



dc. 






dt 



iH(qJ04x3' 
O3 I, 






(67) 



where /3 , v^ are vector standard Brownian motion with unknown variance pa- 
rameters q and cr respectively. The filtering problem is formulated as a stochastic 
disturbance attenuation problem via the Hoo approach where all the intensities q, cj; 
and q are modeled as random non-anticipative processes with finite second-order 
moments on [0, T]. Consider the following estimator: 






-k,Ht 



cjj dt 



(68) 



where Kq and K^ are the matrix blocks of K^ , with the apropriate dimensions. Let 
q^ , Cj denotes the additive estimation error, i.e., q^ = q^ — q^ and c^ = c^ — c^ . 
Given a positive scalar, 7 > 0, we seek for a gain process, {/l^, < i < T}, such 
that the following Hoo criterion is satisfied 



E{ ||qJ|2dt}<72ii;{||qo||2+ / [q^ {i) + <jHt) + aHt)] dt} (69) 
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Whenever Eq. ( |69l l is true, it is said that the L2-gain property is satisfied 
from {qo,<7(i), lit), q;(i)} to {q^c}, < t < T. The augmented process 
{q^ , Cj , q, c} is governed by the following stochastic Ito differential equation 



dq 


u1 




i|2 


t - KgHt - 


l-(q. 


) 







q* 


<£. 




-i^ciJt 


o 







Ct 


dq. 




O 


o 


i|2,-7?,i/, -iS(qJ 




q* 


*t. 




O 





-^cift 




.c* 




04x3 




"04x3" 




" 04x3 " 




f 


Oa 


(7(i)c^t + 


03 

04X3 


a(t)cb/, + 


04x3 

^4^(q*) 


c[(i; 






O: 


J 




4 




A'ciS(qJ. 







df (70) 



where second-order terms with respect to the noises f3 ,v^,ri and to the estimation 
errors q, c have been neglected. Equation Q may be re-written in the following 
compact form: 

ck^ - i^X dt + Gi(ce)a(t)49, + G2«) a (t)div, + G{c[l) a,{t)<in, (71) 

The remainder of the filter development is straightforward and is omitted for the 
sake of brevity. 



5 Numerical Simulation 

Consider a spacecraft rotating around its center of mass with the following time- 
varying inertial angular velocity vector, u>°{t): 



LO°{t) = [1 - 1 l]"^ sin(27rt/150) [deg/sec] 
The measured angular velocity is computed according to 

u}{t) = u:°{t) +qe{t) 



(72) 



(73) 



where e(i) is a standard zero-mean white Gaussian noise, e.g., E{ e{t) e(T)^} = 
1^5{t — t). Typical values of low-grade gyros will be used in the ensuing, i.e., 
q € [10"'^, 10~^] [rad/ y/sec\ . A continuous-time single line-of-sight measurement, 
b(i), is assumed to be acquired. It is computed via the classical vector measurement 
model: 



h{t) = A[q(t)] r(t) + q db{t) 



(74) 



where d3{t) is a zero-mean white Gaussian noise and E{ Sb{t) 5)(t)-^} ~ {I^ — 
b \i^)5{t — t), which stems from the unit norm property of b. The time history of 
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the reference line-of-sights, r{t), is chosen arbitrarily at this stage of the simulation 
study. Typical values of coarse and fine attitude sensors will be used in the ensuing, 
i.e., q; G [10^^, 10^^] [rad]. The simulated sampling time for the gyro and atti- 
tude sensor (continuous) processes is At = O.lsec. The true initial quaternion is 
q(0) = [1, 1, 1, l]^/\/4. The filter is usually initialized with q(0) = [0, 0, 0, 1] and 
P(0) = ^(0) = 10 /^ unless stated otherwise. Monte-Carlo simulations (50 runs) 
were run over time spans varying from 500 to 6000 seconds. This shows the esti- 
mation performances over several periods of the angular velocity dynamics, up to a 
typical revolution time of a Low-Earth-Orbit satellite around the Earth. For the pur- 
pose of comparison, the quaternion Kalman filter of [5:], which will be referred to as 
QKF (where the linear measurement model (l73l l is used), and a typical Multiplica- 
tive EKF [31, referred to as MEKF (where the nonlinear measurement model (l74l l is 
linearized) were also implemented. The novel stochastic Hoc quaternion estimator 
will be denoted as QHF. 

The performances of the quaternion Hoo filter that achieves a 7-level attenuation 
from the intensity q (and the initial estimation error) to the estimation error were 
presented in Q- 

Unknown gyro noise and attitude sensor intensities, q and q 

Next, we summarize the performances of the quaternion Hoo filter that achieves 
disturbance attenuation from the unknown q and q (and the initial estimation error) 
to the estimation error. Extensive Monte-Carlo simulations were run in order to 
compute the actual attenuation ratio AR(T) at the final time T, with T = 500 sec. 
The ratio is defined as follows: 

E{j;^\m)rdt} ^^^^ 

EmiOW+J^iq^+q^)dt} 

where the integral is numerically computed using a time step At = 0.1 sec, 
and the expectations are computed as MC averages. Table [T| shows values of the 
AR{500sec) for various values of q and q . Also, the MC means of the best guar- 
anteed level of attenuation, '^qhp, were computed along the filtering process. Ta- 
ble [T| shows their values in steady-state, which is in general reached after 100 sec. 
Disturbance attenuation ratios from 0.45 down to 0.06 are achieved by the Hoo fil- 
ter. The filter has a better (attenuation) efficiency for higher levels of disturbances. 
For small noise intensities, the AR{500) are similar and show a strong variation 
in the range of (10~^, 10~^). This hints at the fact that the Hoc filtering approach 
can be exploited when using low-grade sensors, which are characterized by high 
levels of noises. As expected from the theory, the AR{500) are all smaller than the 
corresponding guaranteed levels of attenuation, i.e., AR{500) < I'njjp- The dis- 
crepancy between both quantities, however, depends on the noise intensities. For 
instance, when (q-, q;}) are equal to (0.1, 0.1), the actual attenuation ratio (0.08)is 
30 times lower than the guaranteed level. On the other end of the table, in the van- 
ishing noises case, the AR(500) is only six times lower. Therefore, it seems that the 
performance bound is more efficient, e.g., closer to the actual performance, for low 
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noise intensities. Moreover, one can observe that the maximal values for Jq^p as 
well as for the attenuation ratio AR{500) occurs when q and cr are both zero. This, 
indeed, stands in good agreement with the analysis previously presented, where the 
case of vanishing disturbances is the worst case scenario for the Hoa filter. 

Figure[T]-a depicts the time histories of the MC-means (50 runs)for AR{t) and for 
^QHF' ^ ^ ^ 2000 sec. The best guaranteed bound, ^qHF is quickly reaching 
a steady-state around 2.25, while the attenuation ratio is more slow to settle on a 
steady-state at 0.06. 




^^^^ 



(a) AR(t) and -y^up (b) 50 MC-mean and MC-cr 

Fig. 1 Performances of the QHF filter. 50 MC runs, {q, q,) = (0.001, 0.1). 

Figure [il-b presents the time histories of the MC-means and MC-standard devi- 
ation envelop of the angular estimation error, S(p. Albeit oscillating with an am- 
plitude of 0.06 deg around 0.08 deg, 6(p shows good performances: recalling the 



Table 1 Attenuation Ratios AR{500sec) (50 MC runs) between the quaternion estimation 
error, q, and the disturbances {q(0),(j", q} for various values of the intensities {q, q}. 
(Value of the steady-state MC-mean for the smallest reachable Jqhf^ ^s computed in the 
filter.) 



1 [rad] 





10-^ 


10-2 


10-1 





(2.89) 

0.45 


(2.89) 

0.45 


(2.79) 

0.44 


(2.32) 

0.06 


10"^ 


(2.89) 

0.45 


(2.65) 

0.45 


(2.60) 

0.44 


(2.32) 

0.07 


10-2 


(2.78) 

0.44 


(2.53) 

0.43 


(2.46) 

0.41 


(2.31) 

0.07 


10-1 


(2.41) 

0.16 


(2.40) 

0.15 


(2.39) 

0.15 


(2.25) 

0.08 
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Table 2 Time-averages (and time standard deviations) of the quaternion estimation errors in 
QHF (right) and in MEKF (left) for various values of the intensities {q, q; }. Single run. The 
final time is 2000 sec. 



% [rad\ 


10-=* 


10-2 


10-1 




MEKF 1 QHF 


MEKF 1 QHF 


MEKF 1 QHF 


10-^ 


(l.2 10^^ 1 1.4 10^^) 

3 10-^5 12 10-^ 


(l.O 10^^ 1 1.4 10^^) 

0.003 1 0.003 


(0.015 0.080) 

0.020 0.017 


IQ-^ 


(2.1 10-'* 1 1.4 10^3) 

5.110"^ 1 1.310"^ 


(9.0 10"*' 1 1.4 10-3) 

0.008 1 0.007 


(0.015 10.080) 

0.020 10.016 


10-* 


(l.2 10^* 1 1.4 10^^) 

1.5 10-^5 11.3 10-^ 


(2.0 10^3 1 2.1 10^3) 

0.0001 1 0.0003 


(0.015 1 0.082) 

0.020 0.015 


IQ-^ 


(l.2 10-^ 1 1.4 10"^) 

7.9 10-^1 2.0 10~^ 


(3.710^3 1 1.2 IQ-^) 

0.0196 1 0.0008 


(0.015 10.081) 

0.020 1 0.002 


10-^ 


(l.6 10~^ 1 2.0 10"^) 

1.310-'5|1.210-^ 


(0.0120 1 0.0124) 

2.810-^11.410-* 


(0.019 1 0.080) 

0.0201 8.4 10-* 


10-1 


(l.6 10~3 1 1.6 10-^) 

4.8010-^13.3010-'^ 


(0.0172 1 0.0192) 

0.0001 0.0001 


(0.116 10.084) 

2.6010-3 6.80 10"* 



measurement noise level a^ , equal to 5 deg, the QHF provides an attenuation ratio 
of order 0.02 ~ 1/50. 

Extensive simulations were performed in order to compare the performances of 
the QHF and of a standard quaternion Multiplicative EKF (MEKF). In the MEKF, 
the measurement equation model, which is quadratic in q^ is linearized around the 
estimated trajectory. The MEKF is matched to the true levels of noises. For com- 
parative purposes, we consider the additive estimation errors in both filters. Table[2] 
shows the time averages, computed on single runs of 2000 sec, of the quaternion 
estimation error norm in QHF (right) and in MEKF (left). In addition, the values of 
the time standard deviations are provided for both filters (in parenthesis). In gen- 
eral, both filters provide similar time averages of the estimation error. If q, remains 
constant and q is increased, then no significant changes appear. However, when 
increasing a^ , while keeping q constant, there is a proportional increase in the time- 
average of the error. As expected, the MEKF provides smaller standard deviations 
of the quaternion error than QHF, since MEKF approximates the minimum vari- 
ance estimator. Increasing q, while keeping q constant, does not affect the perfor- 
mances (standard deviation's level) of QHF. On the other hand, the dispersion of the 
estimation error in MEKF grows proportionally with q. The same fact can be ob- 
served when increasing q while keeping q constant. This property goes is readily 
explained via the approach used for the filter's development. 
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(a) QHF Vs. Matched MEKF. MC-a. 
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(c) QHF Vs. Unmatched MEKF Case A. 
MC-means. 



(b) QHF Vs. Matched MEKF MC- 
means. 
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(d) QHF Vs. Unmatcfied MEKF Case B. 
MC-means. 



Fig. 2 Quaternion estimation errors in QHF (dashed blue) and in MEKF (full red). 50 MC 
runs, (g, c^) = (0.01,0.1). 



Additional Monte-Carlo simulations were performed for specific levels of the 
noises. Figure ^a depicts the MC-standard deviations of the additive quaternion 
estimation errors in the QHF and in the MEKF, for the case (ct, a^) — (0.01,0.1). As 
expected, the values of a in the MEKF (around 2 10^^ are significantly lower than 
in the QHF (around 2 10^^. This is consistent with the property that the MEKF is 
approximating the minimum- variance estimator. Figure|2l-b shows the time histories 
of the MC-means of the quaternion estimation errors, for {q, ) = (0.001, 0.1). The 
errors appear to be unbiased in both filters. The MEKF plots, however, are smoother 
than those of the QHF. Further, we tested both filters in cases where the true levels 
of noises where not accurately known. This may be the result of on-board failures 
of gyros or attitude sensing devices, which, while undetected, render the MEKF 
unmatched and probe to divergence. In Case A, the filter level of q; was fixed to 
ten times its true value. As a consequence, as can be seen from Fig[2l-c, the MEKF 
is very slow to converge. On the other hand, the QHF, which design is essentially 
independent from the actual values of the noise intensities, produces means that are 
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close to the levels obtained in the previous simulation (Fig. |2]-a). In addition, we 
tested in Case B an unmatched MEKF where the filter a is ten times lower than the 

h 

true one. As a result, the estimation errors in the MEKF converge quicker but to a 
noisier steady-state, which is expected from the Kalman filtering theory. The QHF, 
on the other hand, provides essentially the same performances as in Case A. The 
slight differences arise from the fact that the QHF implement data that are noisy, 
and, thus, the QHF performances are indirectly affected by the level of noises. 

6 Conclusion 

In this work, a novel quaternion attitude estimator was developed in the framework 
of stochastic iJoo filtering using gyro measurements and line-of-sights (LOS) mea- 
surements. The proposed design hinges on a model where the white noises in gyro 
and the LOS measurements have unknown possibly random intensities. The case of 
quaternion and gyro drift Hoo estimation was also addressed analytically. The esti- 
mator computes a quaternion while attenuating the effect of the noise intensities on 
the estimation error. The iJoo filter involves the solution of a set of (differential) lin- 
ear matrix inequalities, which do not depend on the estimated quaternion. Extensive 
Monte-Carlo simulations were run, for the case of guro white noise, showing that 
the proposed filter performs well from the standpoint of attitude estimation, per se, 
in a wide range of gyro and LOS intensities. The guaranteed disturbance attenuation 
level seems to be slightly dependent on the noises intensities, which may be due to 
the fact that the filter's parameters are noisy. The actual level of disturbance attenua- 
tion between the noises levels and the estimation error is improving when the noises 
levels increases, and is minimal for (ideal) noise-free sensors. This was expected by 
the analysis and illustrates the conservative nature of the Hoo filter. When compared 
with two different matched quaternion Kalman filters (KF), the Hoo filter produces 
higher Monte-Carlo standard deviations of the estimation error, but lower Monte- 
Carlo means. The higher the level of noises are, the less obvious the advantage to 
the Kalman filter is. Besides, as opposed to standard quaternion Kalman filters, this 
filter's gain process can be computed independently from the quaternion estimate 
process, which makes it insensitive to estimation errors. This nice feature is fur- 
ther emphasized when comparing the Hoo performances with those of unmatched 
Kalman filters. When provided with too high or too low noise covariances, the KF is 
outperformed by the Hoo filter, which delivers essentially identical levels of errors 
within a wide range of noise intensities. 
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Advanced Optical Terrain Absolute Navigation 
for Pinpoint Lunar Landing 



Marco Mammarella, Marcos Aviles Rodrigalvarez, Andrea Pizzichini, 
and Ana Maria Sanchez Montero 



Abstract. Pin-point landing can only be achieved developing precise Absolute 
Navigation systems. Craters, for their intrinsic properties, are one of the most 
suitable and robust features identifiable in lunar landscape. The Optical Terrain 
Absolute Navigation (OTAN) system provides absolute navigation features and is 
composed by two main parts: the off-line part, focused on the extraction of the 
Landmark Database; the on-line part instead is focused on the Real Time 
identification of the craters and the Orbit Determination process. The presented 
vision-based approach uses Real-Time crater identification in order to extract 
relevant features from on-board captured images of the lunar surface. The detected 
craters are fitted with ellipses and matched to a Lunar Crater Database previously 
created. The matching of the two sets permits the computation of the absolute 
position of the camera. A Kalman Filter uses this information and the IMU 
measurements in order to provide precise complete state space information of the 
vehicle. In this paper, a detailed description of the complete structure of the 
Optical Terrain Absolute Navigation system based on craters detection and 
recognition is provided. 

1 Introduction 

One of the most interesting challenges in descent and landing missions is PinPoint 
Landing (PPL). Considering a probe landing on a planet, the accuracy level that is 
nowadays guaranteed, in terms of distance of the achieved landing site with re- 
spect to the target site, is not smaller than the order of magnitude of tens (or even 
hundreds) of kilometers [1]. Instead, PPL requires a few hundreds of meters wide 
error ellipse, that is to say about two-three orders of magnitude smaller than the 
one achieved. 
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Various PPL solutions have been proposed in recent years for example: 
VISINAV (vision aided inertial navigation system), which its main drawback is its 
high memory requirement due to the usage of image correlation in finding mapped 
landmarks [2]; in [3], authors use SIFT features (Scale Invariant Feature Trans- 
form) as mapped landmark to estimate the spacecraft's global position. SIFT fea- 
tures only prove to be invariant to an affine transformation in illumination change. 
Thus, they are quite vulnerable in case of important illumination changes between 
the orbiter and descent images, such as caused by the difference of the sun's direc- 
tion. [4] relies on a Lidar sensor and matching of landmark constellations. Where 
either maximum or minimum of the surface's height and slope are extracted as 
landmarks from the surface elevation image provided by the Lidar sensor. The 
metrics distances of a landmark with its neighbors are used as it signature. Then a 
matching is performed between the surface and the DEMs landmarks. The main 
problem to this solution is that the system requires the resolution of the surface 
image obtained with the Lidar sensor to be equivalent to that of the map image. 

In [5], modification Harris corner detection is used to extract landmarks from 
orbital and camera images. Landmarks from orbital images are stored onboard in a 
database. The two sets of landmarks are matched through constellation method 
and the absolute position is extracted. The main drawback of this method is the 
requirement of prior orbital images with similar environmental conditions (and not 
yet clearly specified). Furthermore, the number of stored landmarks and associate 
information can be a serious issue to handle since in each image a relatively big 
number of landmarks need to be stored. In order to rapidly access the database in 
real time, the database must be handled carefully. 

All of the PPL solutions have at least the following functionalities: landmarks 
extraction, landmarks matching and position estimation. The present paper pre- 
sents a complete description of an Automated Lunar Lander system, which has 
been designed to be compilable with different solutions but has been implemented 
using craters as landmarks. 

Craters have been claimed by several authors [5] to be excellent landmarks due 
to its scale, rotation and illumination invariance properties. Crater shapes com- 
monly correspond to a known geometric model (ellipse), invariant to scale 
changes and orientation between the spacecraft camera and the surface. The main 
drawback of craters, instead, is the not easily method of identification especially 
when the Image Processing algorithm has to deal with irregular and overlapped 
craters. In the developed method these issues are limited through appropriate 
pre-filtering function and discarding ambiguous craters directly in the database 
creation. 

Optical Terrain Landmark Navigation (OTAN) is an absolute navigation sys- 
tem conceived to perform PPL in a fully autonomous way. It exploits optical navi- 
gation techniques in order to support and, if necessary, to correct, the propagation 
data of an Inertial Measurement Unit (IMU) in conjunction with Ground Tracking 
(GT) measurements. 

The present paper describes the complete structure of the OTAN applied to the 
ESA NEXT-MOON [6] mission. In particular, after giving an overview of the 
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mission scenario and explain the necessity of optical navigation, a complete de- 
scription of the OTAN system is provided. 

2 Mission Overview 

The ESA NEXT-MOON mission [6] is the mission scenario in which OTAN 
technology is applied and tested. A typical GNC design for the NEXT-MOON 
Lunar Lander mission may consider the Descent and Landing trajectory as divided 
in four principal phases: Orbital, Main Braking, Visual and Terminal Descent 
Phases. A synthetic overview of the four phases is given in Table 1 . 

Table 1 Mission Overview 



Phase name 


ORBITAL 


MAIN 
BRAKING 


VISUAL 


TERMINAL 
DESCENT 


LLO 


DESCENT 
ORBIT 


Characteristics 


Free- 
flight 
orbit. 


Free- 
flight 
orbit. 


Maximum 
thrust. 


Landing site 
inside FOV, 
reduction of 
thrust level. 


Pure vertical 
descent 


Altitude range 
(approximate) 


100 km 


100 to 15 

km 


15 to 4-3 km 


4-3 km to 
15-5 m 


15-5 m- until 
Touchdown 



It can seem obvious that the most critical phases in a PPL mission are relative 
to visual and terminal phases. This is actually only partially true because if exces- 
sive dispersion in the navigation solution is accumulated in the first phases, these 
errors cannot be recovered during the last phases because of mass-budget and ma- 
neuverability limits and, also, due to the lack of a system able to recognize the 
landing site [6]. These limits are such that the only way to achieve PPL is to limit 
in all phases the dispersion as much as possible. The here described OTAN system 
has been conceived for providing a precise navigation solution during the orbital 
and main braking phases. It has to permit to reach the beginning of the visual 
phase well below the typical dispersion due to inertial navigation. 



3 Optical Terrain Absolute Navigation System 



Figure I shows the proposed Optical Terrain Absolute Navigation system able to 
recognize craters. The sources can be either geo-referenced images or DEMs. 
From these sources, the landmark extraction shall extract the coordinate and some 
geometrical properties of the craters off-line. Consequently, the resulting Land- 
mark Database will be extracted off-line and stored on-board in order to be used. 
It is required that the spacecraft mounts a Camera providing image data, a Star 
Tracker providing the attitude data and IMU providing acceleration data. The im- 
age will be processed by the Lander Acquired Image Landmarks Extraction func- 
tion, which is an IP function able to extract the craters on-line. This function 
can use data from the navigation filter in order to reduce computational time. The 
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Database projection function uses attitude data and position estimation data pro- 
vided by the filter in order to predict wiiicii landmarks are expected to be seen in 
the current image. The landmark matching function finds the correspondence be- 
tween the Lander acquired image landmarks and the expected Landmarks. The 
Absolute Position Estimation uses the knowledge of the attitude data, Landmarks 
Database and matched landmarks for computing Absolute Estimated Position. The 
matched landmarks are then checked by the Landmark Integrity function in order 
to select the most appropriate landmarks that have to be used for the position 
computation. The position is next provided to the Navigation filter with the land- 
mark integrity in order to estimate the complete state vector. In next sections, each 
block of the OTAN system is described in more detail. 



Georeferenced 
Images 




■* 










DEM 






Fig. 1 Optical Terrain Absolute Navigation System 



4 Landmark Database Creation 



Creating a reliable landmarks database is a fundamental step in the system because 
the spacecraft positioning will be relative to this database. However, after decades 
of research and manual efforts, only tens of thousands of the millions of craters on 
the Moon have been catalogued, mainly those with diameters > 2 km. Automated 
techniques for crater detection and cataloguing are therefore necessary. They take 
advantage of the vast quantities of remotely sensed data now available, especially 
now that precise 3D information is becoming available [7]. 

Crater detection techniques in DEMs are not very different from those em- 
ployed in optical images (described in the following section). The edge detector 
is replaced with an extractor of high slope areas, whose binary output feeds the 
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feature extraction stage. Template matching or voting schemes are the typical op- 
tions for this second step as other alternatives that take into account the lighting 
effects cannot be considered for elevation maps. 

Crater rims in a DEM can be effectively marked by searching for the zero 
crossings of the second-order derivative. After applying this process, the zero- 
crossings are identified and those above a certain threshold are marked. 

The feature extraction stage mainly consists in performing the circle detection 
of the image obtained in the previous stage. The circle finding has been broken in 
two parts: center detection and radius estimation. A final step of ellipse fitting and 
refinement in the DEM image domain is recommended to fine tune the estimated 
parameters, deal with elliptical craters and remove errors introduced in the previ- 
ous steps. When optical images corresponding to the DEM are also available, a 
parallel branch can be added to the system to extract a new set of craters and 
match its output with the one coming from the DEM. Figure 2 shows the results of 
this process in a section of the DEM obtained by LRO [7] and the visualization of 
a portion of the DEM using PANGU [8] with the obtained craters. 




Fig. 2 LRO DEM [7] and identified craters (left), projection of the craters of DEM section 
using PANGU (right) 



5 Lander Acquired Image Landmarks Extraction 



The Lander Acquired Image Landmarks Extraction [9] [10] has been designed fol- 
lowing the approach presented in [1][11] and summarized in Figure 3. Namely, 
three stages can be identified in the algorithm: Border Detection (divided in Edge 
Detection and preliminary crater rim selection), Rim Grouping (composed of cur- 
vature check and crater border coupling) and Ellipse Fitting. 
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Fig. 3 Lander Acquired Image Landmarks Extraction scheme 



5.7 Border Detection 



The first step of the Border Detection process is the Edge Detection. The Edge 
Detection is a standard image processing technique [12] by means of which object 
outlines are identified in an image. Considering a picture as a brightness function 
depending on the two spatial coordinates (vertical and horizontal), edges can 
be defined as local maxima of the just mentioned function. The concept can be 
quantified recurring to the gradient of the brightness function, giving two kinds of 
information: 

1) Gradient magnitude. Given a generic picture-element (pixel), higher is 
the gradient magnitude higher is the probability to encounter an object 
border. 

2) Gradient direction. It gives information about the border direction, which 
is always perpendicular to the gradient vector. 

Edge detection is the basis of the following steps. In fact, the aim of the whole al- 
gorithm is to identify the crater shapes among the other terrain features. Recogniz- 
ing an object means, in this case, to associate its outline to a mathematical model, 
like an ellipse. Therefore, Edge Detection gives a first set of borders, in which the 
crater rims have to be selected among the other surface characteristics. Several 
different techniques are available in literature. The Border Detection process uses 
Sobel edge detector, which allows a sufficient level of accuracy with a non- 
excessive computational load. 

The Edge Detection output image is a binary image in which each identified 
edge pixel is set to the '1' value. Non-edge pixels are, instead, set to '0'. In gen- 
eral, apart from edges actually belonging to craters, many other object outlines 
(e.g. mountains) are detected by the Edge Detection algorithm. The next step of 
the Lander Acquired Image Landmarks Extraction process operates a selection in 
order to keep as many crater pixels as possible, discarding the ones belonging to 
other objects. Craters, due to their particular geometric shape, have also a well- 
defined illumination profile, which can be exploited in order to operate the 
abovementioned pixel selection. As can be seen in the left part of Figure 4, the 
regions with the highest contrast are the ones perpendicular to the light direction 
(directed horizontally from left to right in the Figure). Moreover, considering 
the crater edges, such regions present themselves as curved segments, easily rec- 
ognizable by the algorithm. The preliminary crater rim selection consists in the 
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Fig. 4 T5'pical crater illumination profile (left), overview of the border detection process 
(right) 

evaluation, at each edge point identified by the Edge Detector, of the gradient di- 
rection (perpendicular to the edge direction) with respect to the illumination unit- 
vector. 



5.2 Rim Grouping 

After performing the Border Detection stage (Right part of Figure 4 gives an 
overview of the whole process), crater edges present themselves as typical curve 
segments that are called crater signatures. Notice that each crater is characterized 
by a couple of such segments, one related to the lit side and the other one to the 
shaded side (see Figure 4). Both sides of the crater signature are to be taken to- 
gether in order to be successfully fitted by the ellipse-specific fitting algorithm. 
This can be quantitatively translated in assigning a single label value to the pixels 
belonging to the whole crater signature. It is what is performed by the Rim Group- 
ing process, which is divided in the Curvature Check and Crater Border Coupling 
steps [9] [10]. The output sets of rim grouped are provided as input to the Ellipse 
Fitting. 

5.3 Ellipse Fitting 

Ellipse Fitting is the last stage of the Lander Acquired Image Landmarks Extrac- 
tion. It applies an ellipse-specific fitting algorithm [13] to the border couples pro- 
vided by the Rim Grouping block. Each crater candidate is fitted with an ellipse in 
order to take into account off-nadir Camera attitudes, which cause craters to be 
represented in the image as ellipses instead of circles. Although two selection 
checks have been performed in the previous stages, some non-crater borders can 
be mistakenly considered as crater signatures. In particular, false detections may 
present very high eccentricities. Therefore, a first check is done discarding too ec- 
centric ellipses. Moreover, a further and more accurate check is performed using 
the fitting error and discarding the ellipses, which are characterized by the worse 
fitting. At the end of the process, each crater is associated with five parameters 
(i.e. the ellipse parameters): 2-D center coordinates, semi-axes lengths (both major 
and minor) and orientation. The list of crater ellipse parameters is the final output 
of the Lander Acquired Image Landmarks Extraction process. Figure 5 shows cra- 
ters detected in images captured from LRO and Kaguya. 
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Fig. 5 Craters Detected in images captured from LRO (left) and Kaguya (right) 

6 Database 2D Projection 

The Database 2D projection algorithm projects the Landmark Database craters 
generating a crater distribution as would be seen by a hypothetical camera sharing 
the Lander attitude and estimated position. The output of the algorithm is a list of 
parameters, similar to the one generated by Lander Acquired Image Landmarks 
Extraction process. In fact, the list created by the Database 2D projection can be 
considered as the output of a "perfect" Lander Acquired Image Landmarks Extrac- 
tion process if the estimated position and attitude were correct. 



7 Landmarks Matching 

Once the output of Lander Acquired Image Landmarks Extraction and the output 
of the Database 2D Projection are available, the problem becomes a typical Point 
Matching problem [14] where two sets of points with different dimension have to 
be matched together. In this case, four-dimensional Point Matching (PM) can be 
applied in order to give more robustness to the solution. The four dimensions 
available to the process are the two coordinate in the camera plane and the length 
of the both semi-axes of the ellipse for each crater, in substitution the orientation 
of the crater can be used instead the length of one semi-axis. The process of Point 
Matching has been extensively described in [14]. 

Figure 6 aims to illustrate the complexity faced during the Landmarks Match- 
ing process. The figure shows the analysis performed on a captured 512 x 512 
frame. The black ellipses represent the projection of the database craters, while the 
red ellipses are the craters detected on the captured frame. The landmarks match- 
ing phase will associate the detected craters (in red) to the database craters (in 
black), using a four-dimensional Point Matching. As a result, the Landmarks 
Matching will return pairs of craters (one from the database and one from the cap- 
tured frame). Matches are marked in the figure as a blue crosses joined by a blue 
line, where the crosses are located in the center of the database craters matched 
and in the center of the detected craters. 
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Fig. 6 Complexity of the Landmarks matching 

8 Landmark Integrity 

The Landmark Integrity function is an ulterior check performed before the Abso- 
lute Position Estimation. Since the Landmark Matching can be wrong, this func- 
tion has the purpose of eliminating false matching or at least informing the Navi- 
gation filter that the new measurement is corrupted. The landmark integrity is 
performed using the Database 2D Projection data and the matched craters, the two 
sets of points are compared using proximity and rigidity criteria. If one or more 
craters are considered as an invalid matching the craters are removed from the 
subset whenever the number of craters is still larger than four. If the number is less 
than four the covariance matrix of the filter will be modified in order to provide 
the filter the information that the actual measurement is corrupted. 




Error to be detected by 
Landmark Integrity Phase 



Fig. 7 Landmark Integrity process 
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Figure 7 shows the operation performed by the Landmark Integrity block. A 
false match can be detected in the figure on coordinates (450,105), this wrong 
matching is detected by the Landmark Integrity algorithm and the matching is de- 
leted in the Absolute Position Estimation process. 

9 Absolute Position Estimation 

The Absolute Position Estimation algorithm uses the information of the Crater Da- 
tabase, the attitude information provided by the Star Tracker and the Landmark In- 
tegrity output for computing the Absolute Position Estimation [15]. A minimum of 
four craters are requested to be provided by the Landmark Integrity process in or- 
der to reach the requested level of accuracy. The output of this block is usually the 
distance of the camera to the origin of the reference frame in which the Crater Da- 
tabase is expressed. Since the position and orientation of the camera in the vehicle 
is known, the vehicle position can be computed. 

10 Navigation Filter 

The navigation filter includes a complete model of the Moon gravity containing 
the central term and the LP 1 65 error gravity model. The filter is a Linear Kalman 
Filter (KF) able to integrate different sensors working with different frequency. 
This is particularly important when the measurement of different sensors have to 
be integrated / fused in order to obtain a more precise quantity. In the following 
figure is represented a schematic version of the Navigation filter. It requires the 
gravity components of the acceleration and the measurements provided by the 
OTAN system and the IMU. The sensor weight logic decides how much and when 
a sensor has to be trusted. Namely, it receives information of the Landmark integ- 
rity in order to transmit the information that the measurement can be corrupted or 
if more than four craters are recognized the covariance matrix is properly adjusted. 
In addition, when the vehicle is in the orbiting phase the accelerometer is dis- 
carded from the measurement whenever the acceleration is inside a certain thresh- 
old derived from the IMU noise parameters. 
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The Kalman gain Kf is computed applying the formulas described in [16]. The 
matrixes Q and R used in the Kf computation contain the covariance of the state 
and measurement noises. Q and R are time variable and they determine when each 
sensor has to be considered and how much it has to be trusted. The initialization of 
the filter is performed using Ground Tracking measurements and the filter has to 
be able to start from almost lost in space condition. 

11 Conclusion and Future Works 

This paper has presented the Optical Terrain Absolute Navigation (OTAN) system 
being developed at GMV. Different parts of the system have been tested in simpli- 
fied simulation environment [9] [10] providing encouraging results in line with the 
expected accuracy. In particular, the on-line part of the system has shown robust- 
ness to illumination condition and to error in attitude less than 0.1 degrees. Infor- 
mation derived by the mentioned analysis has been used in an end-to-end GNC 
simulator showing that pinpoint landing is only achievable when precise absolute 
navigation is present in the orbital and main braking phases of the mission [6]. 

Extensive test campaigns are planned to be performed in the close future in the 
frame of ANT ARES project, where the presented system will be extensively com- 
pared with [5]. In the project both systems will be adapted to a common architec- 
ture and the same test will be performed in both systems making the comparison 
straightforward. The objective is to bring the technology to TRL 4. 
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Methodologies to Perform GNC Design and 
Analyses for Complex Dynamic Systems Using 
Multibody Software 

G. Baldesi, T. Voirin, A. Martinez Barrio, and M. Claeys 



Abstract. Virtual simulation is currently a key activity that supports the specifica- 
tion, design, verification and operations of space systems. Taking advantage of 
dynamics research activities, ESA has currently been developing a multibody 
software (DCAP) together with industry for more than 30 years heritage in space 
applications. This software is a suite of fast, effective computer programs that pro- 
vides the user with capability to model, simulate and analyze the dynamics and 
control performances of coupled rigid and flexible structural systems subjected to 
possibly time varying structural characteristics and space environmental loads. 
With the implementation of dedicated interfaces to other specialised software 
(such as NASTRAN, CATIA or Matlab/Simulink), it is possible to reproduce, 
with a quite good level of details, most of the key subsystems (such as trajectory, 
structures, configuration, mechanisms, aerodynamics, GNC and propulsion) of the 
launch vehicle and/or spacecraft in a single simulation. 

In this paper, an overview of two GNC studies, a generic launch vehicle feasi- 
bility study and a spacecraft with no-conventional configuration, is presented. 



1 Introduction 

Rigid modelling of systems has proved to be efficient in many applications for 
AOCS/GNC designs. Nevertheless, in some specific cases, such as high resolution 
imaging satellites, high accuracy pointing space telescopes, or launchers, the flexi- 
ble dynamics content of the system become a driver for the AOCS/GNC design, 
requiring ad-hoc dynamics model at the very first start of the project (feasibility 
phase, or phase 0). 

To cope with this problematic, ESA/ESTEC has developed an in-house capabil- 
ity and methodology aiming at improving the flexible dynamics modelling and the 
subsequent GNC design. The purpose of the paper is to explain this strategy and 
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its adaptation to two real applications, namely the generic launcher, and the IXO 
X-ray space telescope, focusing more on the tools and methodology adopted in 
that frame than in the actual discussion of the results. 

2 Multibody Dynamics in Aerospace Applications 

In recent years, space-system design has shown a clear trend towards increasing 
complex configurations. Typical examples are the use of several flexible compo- 
nents (antennas and solar arrays), the need for deployment and retrieval mecha- 
nisms, the demand for high precision pointing systems, and the increase in mission 
scenarios implying the assembly of large structures in space. This trend has also 
caused an evolution towards a multi-disciplinary design approach, particularly in 
the area of dynamics and control [1]. 

In order to study the performance of generic controlled dynamic systems, it is 
essential to have a dedicated tool, which allows the user to model, in a short time, 
the complex behaviour of the dynamic systems and their interactions with the con- 
trol. In fact, some systems require a model with more than one body in order to 
take into account their different characteristics and their mutual dynamic interac- 
tions. This task is complex and requires dedicating some time to understand, code 
and validate the dynamic behaviour of the system. 

A lot of research has gone into the development and improvement of multibody 
software, with the aim of reducing the time of modelling a system and the compu- 
tation time required to run an analysis. Multibody software involve the derivation 
of the equations of motion for multibody systems, which are systems characterized 
by several bodies connected by hinges that permit relative motion across them. 
Robots, launchers and spacecraft including articulated appendages like solar ar- 
rays are typical examples of this kind of complex dynamic systems. 

ESA has a long experience in using multibody software in order to analyse 
complex dynamic systems [2]. For this purpose, several software (such as DCAP, 
Adams and Samcef/MECANO) are currently used in different studies [3]. 

2.1 DCAP Software 

Early approaches to the dynamics formulation for multibody systems lead to the 
equations of motion, for open-loop tree topologies, of the form: 

where, M is an (n x n) mass matrix, q = [q q^ ... q"f is an (n x 1) column matrix 
representing the generalised coordinates and F is the column matrix containing the 
contributions from centrifugal, Coriolis and external forces. 

For a numerical simulation of such a system, the mass matrix must be inverted. 
Since the inversion of an (n x n) matrix involves operations proportional to the cu- 
bic power of n, this is called an Order(n ) approach. As the number of degrees 
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of freedom increases, this matrix inversion for every integration step, becomes 
computationally expensive. Thus, researchers have sought methodologies to cir- 
cumvent the mass matrix inversion and to improve computational efficiency. The 
research into improvements in formulations that increase computational speed re- 
sulted in - what are today called - Order(n) algorithms. The reason for this nomen- 
clature is that the computational burden in these schemes increases only linearly 
with n. More details have already been presented in [4]. 

DCAP, which has currently been used in the Structures & Mechanisms Divi- 
sion at ESTEC (ESA), has the possibility to simulate the same problem perform- 
ing the analysis based on Order(n3) approach, derived by Lagrange method, and 
on Order(n) one using Kane's method of generalized speeds. 

The development work that was undertaken during years by Alenia Spazio un- 
der ESA contract, culminated in the production of DCAP the Dynamics and Con- 
trol Analysis Package [5]. DCAP is a suite of fast, effective computer programs 
that provides the spacecraft analyst with a powerful tool for designing and verify- 
ing the dynamics and control performance of coupled rigid and flexible structural 
systems. For the latter a dedicated symbolic manipulation pre-processor, used in 
the coding optimisation, has been coded in order to compute the minimum set of 
equations of motion for each particular problem. 




Pre Flex 

Flexible data pre-processor 
• A5KA.DCAP-NASTRAN Intertact 



Initialise Orbit 

Orbital Data Inltlallsatloi 



r- 




— s 






Time-domain 
programs 


1 












Frequency-domain 
programs 










Trans_Matrix 

Matrix Quadruples Handler 
• MATLAB-DCAP-MATRIXx Interface 




Plot_Results 

Simulation runs 
graphic post -processor 

• Time Plots 

• Frequency Plots 

• Root Loci Plots 

• Harmonic (FFT) Plots 



Animate 

3 animation tool 



J 



Fig. 1 DCAP features overview 



The modelling capability is completed with the possibility of user-defined envi- 
ronment, allowing for modelling of specific control feature not directly included in 
the dynamic package's library. For the latter control modelling, straightforward in- 
terface with Matlab/Simulink exists. 
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Fig. 2 DCAP IF to MATLAB-Simulink (co-simulation) 



3 ESA Launch Flight Dynamics Simulator 



Since 2001, ESA Structures & Mechanisms division (TEC-MS) has dedicated an 
important amount of energy in developing a solid knowledge in launcher flight 
dynamics [6]. Using multibody software, which can model the dynamic behaviour 
of interconnected rigid and/or flexible bodies, each of which may undergo large 
translational and rotational displacements, it is possible to assess and verify per- 
formances at system level. More in detail, launcher flight dynamics analysis al- 
lows to verify whether the selected launch vehicle design is able to accomplish the 
mission objectives taking into account the output information provided by all 
other subsystems such as Trajectory, Structures, Mechanisms, Aerodynamics, 
Propulsion, GNC etc. In particularly, non-linear dynamic simulations have been 
run in order to assess stability in nominal and off-nominal condition, TVC angular 
deflection and general loads. 

Taking advantage of the strong know-how in dynamics, coming from long 
R&D activities in Multibody Dynamics, specific launcher flight dynamics tools 
(VEGA-DCAP-sim in Fig.l) was developed for different ESA projects in order 
to investigate not only flight dynamic effects for VEGA Support and CDF LV 
studies, but also local analyses (such as gust response, lift off, multi-payload sepa- 
rations) for GSTP3/GSTP4 Ariane 5 TVC, Swarm Support, Galileo lOV & lOF 
Support, IXV support. Furthermore, multibody software is gaining importance 
also in the difficult area of coupled load analysis [7], because it allows a 
much faster and malleable approach when compared to the classic FEM based 
procedures. 
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Fig. 3 VEGA-DCAP-sim overview 

3.1 What Makes It Unique 

In a nutshell, the main characteristics of ESA launcher flight dynamic simulator 
(VEGA-DCAP-sim) are: 

• Multibody-based (easy to model the dynamics of "complex" launchers) [6] 

• Flexible or rigid bodies both with time varying characteristics [1] 

• LV flight Dynamics-Control interaction (directly imported in Matlab) [1] 

• Environment modelling for Flexible launcher including external disturbances 
[8] 

• Easy modelling non-linear effects (as misalignment, inertia influences, 
crosstalk effects, sloshing.) 

• Easy modelling of dynamic transitions (lift-off analysis, stage and payload 
separations, ...) [8][9] 

• High degree of compatibilities with different software packages. 

3.2 GNC Study Applications 



3.3 Generic Launch Vehicle Feasibility Study 

3.3.1 Launcher Design and Trajectory Optimization 

The conceptual design of an expendable launch vehicle is dominated by interac- 
tions between many engineering disciplines: optimal trajectories, aerodynamics, 
propulsion, structures and mass estimations. All of these disciplines are usually 
highly coupled and several iterations between them are needed to produce a 
complete vehicle design. 
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In the field of trajectory optimization, ESA has developed, improved, and 
commercialized a trajectory optimization tool called ASTOS. The entire optimiza- 
tion scheme used in ASTOS has its foundations in the method of direct optimiza- 
tion which requires a non-linear programming solver (NLP). 
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Fig. 4 Data exchange in ASTOS multidisciplinary design optimization environment 



Recently, the trajectory optimization capability of ASTOS has been enhanced 
to include launch vehicle design optimization. In this way the tool is now capable 
of performing multidisciplinary design optimization (MDO) linking several disci- 
pline models. The data exchange between these disciplines is considerable as 
many disciplines depend on input from many others (see Error! Reference 
source not found.). 

Nevertheless by means of adequate formulation of the problem and 
transcription methods, the optimal design of the vehicle together with the 
trajectory design can be obtained by means of NLP solvers using the MDO all-at- 
once approach. [10,11]. 

As a result the launcher scenario has been extended by a geometry model for all 
stages and the fairing, where lengths and diameters are optimisable. Interstages 
manage different stage diameters, which allow the modeling of hammerhead con- 
figurations. The aerodynamics are computed by Missile Datcom, where the shape 
is used from the geometry model. Tank models for separate, common bulkhead 
and enclosed tanks are provided to support the geometry model and the mass 
estimation. 

The mass estimation is based on regression formulas for most important com- 
ponents. Alternatively, the mass estimation can be refined with a One-Beam Ap- 
proximation (OBAX), which performs a structural analysis based on external and 
internal forces and weights and which results in a minimum wall thickness and 
mass estimation. Finally, the liquid engine model computes the combustion at 
equilibrium conditions with the NASA tool CEA and adds efficiency factors based 
on regression. 
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Fig. 5 Different tank configurations available 

The ASTOS tool has been used in several ESA Concurrent Design Facility 
(CDF) studies improving the design process by obtaining a more realistic initial 
guess for a launcher system design. This initial guess is then used as the starting 
point for more detail subsystem design iterations. Once more accurate data is 
available from all the other subsystem, ASTOS can also be used as a simple tra- 
jectory optimization tool. 

3.3.2 Dynamic Part 

Lately, based on past successful experiments [12], the advantage of multibody 
software has also been made available to ESA Concurrent Design Facility (CDF) 
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studies (Figure 6). Indeed, since CDF applies the concurrent engineering method 
to the design of future space missions [13], the use multibody software is a key 
tool for analysing performances at system level, while taking into account simul- 
taneously several subsystems. In the next figures launch flight dynamics simula- 
tion applications are briefly presented, which highlight the interfaces with other 
subsystems and some typical results. 

Launch vehicle dynamics having multiple noz- 
zles can be easily simulated using multibody ca- 
pabilities with minimal workload. An example is 
reported in Fig.4, which represents an example 
configuration of future launcher study. The first 
stage consists of 4 boosters and a main motor 
controls 5 nozzles, which can be tilted independ- 
ently. Despite being a rather complex to repre- 
sent, the system proved relatively straightforward 
to model using the multibody tool, Therefore, 
more time was invested to investigate the best 
philosophy to command the individual nozzles, 
which could meet the strict requirements placed 
on the control part. 




Fig. 7 Launcher with multiple 
nozzles 



3.3.3 GNCPart 

The first step has been the elaboration of a linear model for a rigid launch vehicle. 
The resulting model is presented hereafter: 
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where U is the launch vehicle flight path speed and A^ represents the perturbed 
pitch angle, i.e. the difference between the reference trajectory pitch angle and the 
actual pitch angle. Similarly, Az represents the perturbed vertical component of 
the LV velocity. W is the wind velocity acting perpendicular to the launch vehicle 
and AS is the perturbed TVC deflection angle, measured with respect to the 
longitudinal axis of the LV. 

Additionally, the different coefficients present in the matrices are detailed here- 
after: 
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Where: 

L^ is the derivative of the lift force with respect to the angle of attack, Q the 

dynamic pressure, S the aerodynamic reference area and CN^ the derivative 

of the aerodynamic normal coefficient (body axes) with respect to the angle of 
attack. 

7"^ is the static thrust, which can not be deflected by the nozzle (applicable for 

boosters with no TVC) 

7"^ is the "controllable" part of the thrust, deflectable by the TVC. 

D is the drag force and U is the velocity of the LV 

m and / are the current mass and moment of inertia of the launcher at the 

>y 

given time. 

^PivotiCoG ^^ ^^ distance from the nozzle pivot point to the current LV center 
of mass. 

^CPiCoG ^^ ^^^ distance from the current aerodynamic center of pressure to the 
current LV center of mass. 

The second step has been to design a linear quadratic regulator (LQR) at differ- 
ent times of the flight, taking into account the time and frequency domain specifi- 
cations. The different linearized models have been computed every 10 seconds of 
the flight. 

Once the different control laws are computed for each time slice, the gains are 
linearly interpolated resulting in the final values shown in Figure 8: 

Finally, the complete GNC system performance has been assessed using the 
DCAP simulator, which takes into account the elastic modes of the launcher. In 
order to reduce the effect of this vehicle elastic dynamics, a low-pass filter has 
been added. 
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LQR Gains vs Flight Time 
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Fig. 8 LQR gains versus time 

As mentioned before, the desired trajectory lias been provided by the tool 
ASTOS. An overall overview of the DCAP simulator is shown in the following 
picture. 
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Fig. 9 Launch DCAP simulator flowchart 



The results, provided in the following Figure 10, shows that the sample 
launcher analyzed is stable, the desired trajectory has been followed within 3.5 % 
of trajectory error and TVC does not exceed the design limit of 6 degrees. 

It should be noted that no active roll control was implemented in the simulation 
presented hereafter. Nevertheless the roll increase occurred during the first stage 
flight of this particular launcher is relatively small and it is not expected to cause 
major concerns in future steps of the control design. 
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Fig. 10 Sample results of the launch vehicle simulator 
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Fig. 11 IXO General Configuration (deployed). ESA CDF design 
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3.4 Spacecraft with No-Conventional Configuration 

The International X-ray Observatory (IXO) is a Large-class mission candidate 
within ESA Cosmic Vision 2015-2025 program, being studied in a cooperative 
way by ESA, NASA and JAXA. Its main feature, which is of interest for this pa- 
per, is the need to extend the telescope structure from about 8 meters, as config- 
ured inside the launcher fairing, up to (at least) 20 meters (to achieve required op- 
erational focal length). Peculiar to the IXO telescope is the need to extend its focal 
length by a large amount (about 10 m) while ensuring extreme pointing accuracy 
and stability of the different telescope modules. The deployment principle is based 
on an innovative "articulated booms" concept, which is currently being investi- 
gated within (and outside) ESA. The elastic behaviour of the booms and the non- 
linear hinge characteristics (friction, backlash, hysteresis ...) are modelled to 
simulate the system deployment dynamics. Application of advanced multibody 
software techniques permit the investigation of coupled structural dynamics / con- 
trol system, aiming at pointing accuracy / stability performance verification. [14] 



3.5 Modelling 

3.5.1 Dynamic Part 

The dynamic behaviour of IXO spacecraft was modelled as a set of interconnected 
rigid or flexible bodies, each of which may undergo large translational and rota- 
tional displacements. The service and the instrument modules are considered as 





Fig. 12 IXO multibody model 
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rigid bodies as well as the motorised and passive hinges. On the other side, the so- 
lar arrays and the articulated booms are modelled as flexible taking advantage of 
the direct interface with MSC.NASTRAN. In addition of the main spacecraft free 
motion in space, "parallel kinematics" approach, uses nine revolute type of joints, 
which can benefit from the utilisation of very well known (to the space commu- 
nity) building blocks. 

In order to provide mathematical models to Attitude and Orbit Control Systems 
(AOCS) and Guidance Navigation and Control (GNC) specialists (TEC-ECN) in 
ESA, IXO multibody model has been slightly modified by including system sen- 
sors (accelerometers, gyros,...), actuators (reaction wheels, thrusters,...) and addi- 
tional output (lateral displacement of the instrument modules with respect to the 
service one, focal length,... ). 

For this purpose two models have been generated in "fully deployed" condi- 
tion: a linear and a non linear model. Taking advantage of numerical linearization 
capability of most multibody software, it is possible to obtain linear model, de- 
scribed by means of the quadruplet (A, B, C, D). This mathematical model can be 
easily imported in Matlab/Simulink environment having as inputs the actua- 
tors/external force & torque disturbances and as outputs the sensor/additional dy- 
namics outputs. On the other side, once the control law has been setup, it is com- 
pulsory to verify whether the assumptions are still validated. Furthermore, a "non 
linear" mathematical model is also generated and exported in Matlab/Simulink 
environment. The nonlinearities might be due not only to stiffness characteristics, 
transition events,. . . but also to the rigid motion of the whole system. 

3.5.2 GNC Part 

The size of the IXO telescope (20m) coupled with the need for relatively high 
agility (85% observation efficiency), and high accuracy pointing (10 arcs APE, 1 
arcs AME around lateral axes), lead to unusual AOCS design, relying on a swarm 
of 5 "big" reaction wheels (150 Nms - 0.4 Nm class) to control the attitude of the 
spacecraft. The coupling of AOCS with flexible structure has to be analyzed care- 
fully considering the very demanding lateral stability required by the payload (0. 1 
mm lateral displacement requirement). 

To perform this assessment in the frame of IXO, a high-fidelity dynamics 
model of the Spacecraft is therefore required to model the flexibility inherent to 
this type of design, and in particular the induced motion of the detector with re- 
spect to the mirror. The DCAP-based linear flexi-body model detailed above has 
been used in that purpose, with as main objective to address the feasibility of and 
to characterize the AOCS performance for the nominal AOCS mode of IXO, that 
is to say the "Fine Pointing and Slew Mode" (EPS). 

Design Approach 

The following step-by-step approach for the control design and validation has 
been adopted: 
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1. First, a simplified dynamics model has been used, namely a cantilever 
beam attached to a rigid body, roughly representative of the expected 
MCI properties of the spacecraft ; a simplified controller has been then 
synthesized to characterize the achievable pointing performance of the 
spacecraft (Proportional Integrator & Derivative PID controller). This 
step is mandatory to initiate the preliminary AOCS design when no dy- 
namics model is yet available. This step enables also to provide to the 
dynamics modelling expert the AOCS actuators characteristics (Reac- 
tion Wheels and Thrusters) and to specify the outputs of the dynamics 
model that are required to characterize the AOCS/GNC performance. 
For IXO these were mainly: 

a. the attitude and attitude derivative of the S/C at mirror node. 
This is obviously required to assess the pointing and pointing 
stability performance. 

b. the 3-axis translation of the detector with respect to the mirror at 
focus point of the telescope. This is required to characterize the 
blurring of the X-ray image induced by the detector relative mo- 
tion wrt mirror. 

2. Once the first high-fidelity Linear Dynamics Model has been made 
available by the dynamics modelling expert, a robust controller has been 
designed, based on the Hinfmity formalism. The use of the linear dynamics 
model at this stage enables to validate extensively the behaviour of the 
control loop on the various IXO operation scenarios. 

3. Finally, the AOCS performance is cross-checked on reference cases us- 
ing the final dynamics model, namely the Non-linear Dynamics Model 
with the robust controller previously designed. 

This approach has many advantages: 

1. The AOCS expert can start working on the design without having the 
fully representative dynamics model, which is by definition often the case 
at the start of a feasibility study. 

2. A first iteration on the AOCS design can be performed ahead, and speci- 
fications for a representative dynamics model can be derived. 

3. When the linear dynamics model is delivered, it is easy to perform a first 
validation of this dynamics model with the simplified model. 

4. The non-linear model enables to validate the performance and robustness 
of the proposed controller. 

This process is summarized in Figure 13 
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Fig. 13 IXO AOCS/GNC co-design process 



AOCS Modes 

The Fine Pointing and Slew Mode (FPS) of IXO includes four different operation 

modes, as identified in Figure 14: 

the Inertial Pointing Mode 

the dithering mode, where the boresight follows a Lissajous pattern 
the Raster mode, where the telescope follows a mosaic pattern 
the Slew mode, corresponding to a retargeting manoeuvre of the tele- 
scope from target n to target n-n 1 . 



Major 
Anomaly 




A': automatic for Wheels off-loading 



Fig. 14 IXO AOCS modes 



Robust controller design 

A single robust controller has been synthesized for the FPS, based on the HinfinHy 
framework. The IXO science requirements have been translated in the standard 
problem description for robust control as described in [15] [16]. The generated 
controller covers the whole operation modes of the FPS, with, for the slew opera- 
tion mode, the use of a feed-forward scheme for the Reaction Wheels command. 
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The synthesized controller has a low bandwidth of 1 mHz to avoid the excitation 
of the S/C flexible modes. Standard robustness criteria of 6 dB margin on the gain 
and 30° margin on the phase have been applied. 

The controller performance has then been assessed with a 3-dof high-fidelity 
relying on the linear dynamics model as described above, with the following 
scenarios: 



> 



> 



Raster pointing scenario: two consecutive steps of 10 arcs around the Y and 

X axis respectively, with 3000s between the two steps. 

Dithering pattern scenario : the Lissajous is 20 arcsec wide, with an X-axis 

frequency of 1/1200 Hz and a Y-axis frequency of 1/1800 Hz 

Slews around X, Y and Z : Amplitudes up to 1 80 ° have been considered to 

cover the whole spectrum of possible slews. 



Results 

The results obtained have confirmed the feasibility of the baseline IXO AOCS de- 
sign, and has even shown that margins with respect to some requirements were 
important. In particular, the lateral stability of the detector platform with respect to 
mirror is always much below the requirement of 0.1mm, with a worst case excita- 
tion by AOCS smaller than 1 |im as shown in Figure 15. This proves that the 
AOCS might have coped with much more flexible (and therefore lighter) designs. 
It shall be noted that higher deviations are expected from thrusters impulses, used 
in TCM (required for example for wheels offloading or orbit control manoeuvres) 
or ASH modes. Future work dealing with the design of the other IXO AOCS 
modes will address this point, which is not expected to be critical, the lateral de- 
viation requirement being not as critical in such phases. 
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Fig. 15 IXO worst case lateral deviation (detector vs Mirror) 180° slew around sun axis 



The overall pointing performance is in line with the 10 arcs APE requirement 
for all the observation modes. The dithering mode required by the Mission Re- 
quirements revealed to be the most challenging one for the AOCS, the frequency 
of the lissajous pattern being of the same order as the controller bandwidth. As a 
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consequence, the robust controller has difficulties in tracking the Lissajous pattern 
as presented Figure 16, with on oscillatory tracking error of -10 arcs amplitude If 
acceptable by the mission, a slight relaxation of the Lissajous frequency would 
solve this issue. 

The slew mode is performed without saturating the wheels, and with a mean 
tranquillization time of -lOOOs (worst case presented Figure 17 is for a 180° 
slew). The tranquillization time has proven to be for IXO a significant contributor 
to the overall observation efficiency, with an impact of -2% on the overall tele- 
scope availability, making at the end the observation efficiency requirement only 
marginally met (86.6% obtained for 85% specified). 

Finally, the maximal agility reached is close to 75°/hr, including tranquilliza- 
tion time. Realistic agility figures are an important input to scientists when estab- 
lishing the telescope observation plan. 
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Fig. 16 IXO Lissajous and Raster guidance patterns (top), and simulated pointing perfomi- 
ance (bottom) 
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Fig. 17 IXO Slew of 180° amplitude wrt X-axis. Pointing error (left) and Reaction Wheels 
angular momentum load (right) 

Conclusion 

Overall, the AOCS analysis performed in the frame of IXO based on the DCAP 
dynamics model, pave the way for an even more ambitious approach for the as- 
sessment and pre-design of flexible spacecrafts: an integrated approach performed 
at phase (CDF) in co-design between structure-mechanisms and AOCS/GNC 
subsystems would enable to improve the S/C design process by adjusting the 
structure flexibility to the AOCS capability, and vice & versa, for given science 
requirements. 



4 Conclusions 



In conclusion, the ESA Launcher Flight Dynamics Simulator represents an impor- 
tant successful story of a European software package, internally developed by the 
European Space Agency with the support of Thales Alenia Space Italy. Its pecu- 
liar generic modelling capabilities and computational speed are strong assets of 
the simulator, enabling real time comprehensive simulation of an entire multi- 
stage launcher along all the different phases of the launch. The high degree of 
interface with different specific tool packages allows for exchanging data and co- 
operating with other disciplines at the same time in a reliable and user-friendly 
environment. 

The International X-ray Observatory spacecraft has been considered as a chal- 
lenging test case to evaluate advanced multi-body simulation packages. The simu- 
lation strategies, tools and main results have been presented in details, together 
with AOCS/GNC interfaces. The use of the DCAP multi-body software has en- 
abled to perform a preliminary design of a robust AOCS design for IXO ; further 
more, the integration of this dynamics model into a 3-dof AOCS simulator made 
possible the end-to-end characterization of IXO AOCS performance for the whole 
science operation mode, proving to be an efficient support for the follow-on of 
IXO industrial assessment studies, as well as a valuable input for ESA internal 
Cosmic Vision L-Class review. 



Methodologies to Perform GNC Design and Analyses 



449 



Acronyms 

AME Attitude Measurement Error 

AOCS Attitude and Orbit Control Systems 

APE Absolute Pointing Error 

ASH Acquisition and Safe-Hold Mode 

ASTOS Aero-Space Trajectory Optimization Software 

CDF Concurrent Design Facility 

CEA Chemical Equilibrium with Applications 

DCAP Dynamic and Control Analysis Package 

ESA European Space Agency 

ESTEC European Space Research and Technology Centre 

FEM Finite Element Model 

EPS Fine Pointing and Slew Mode 

GNC Guidance, Navigation and Control 

GSTP ESA's General Support Technology Programme 

IMU Inertial Measurement Unit 

IOC Initial Operation Capability 

lOV In-Orbit Validation 

IXO International X-ray Observatory 

IXV Intermediate Expermental Vehicle 

JAXA Japan Aerospace Exploration Agency 

LQR Linear Quadratic Regulator 

LV Launch Vehicle 

MDO Multidisciplinary Design Optimization 

NASA National Aeronautics and Space Administration 

NLP Nonlinear programming solver 

OBAX One-Beam Approximation 

PID Proportional Integrator & Derivative 

R&D Research and Development 

S/C Spacecraft 

TCM Thrusters Control Mode 

TEC-ECN ESA's GNC Section 

TEC-MS ESA's Structures & Mechanisms Division 

TVC Thrust Vector Control 
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Optimal Guidance and Control of Lunar 
Landers with Non-throttable Main Engine 



Thimo Oehlschlagel, Stephan Theil, Hans Kriiger, Matthias Knauer, 
Jan Tietjen, and Christof Biiskens 



Abstract. Autonomous soft, safe and precise landing on celestial bodies like the 
Moon, planets and asteroids is still a challenging task for future exploration 
missions. To achieve a maximum of payload mass landed on the target body the 
trajectories of landing vehicles need to be (fuel) optimized. In order to allow an ad- 
justability of the trajectory and a compensation of disturbances for all vehicles so 
far a thrust modulation is required. The drawback is that currently no main engine 
is available which allows the needed thrust modulation for an efficient, robust and 
safe landing on a celestial body like the Moon. The technology of the Apollo mis- 
sions is not available anymore. Most planned lunar missions rely on the modulation 
capability of multiple engines where in some cases the thrust of the auxiliary en- 
gines for modulation is in the order of main engine thrust. This approach requires 
a large number of smaller engines to achieve the needed thrust modulation adding 
complexity and increasing the probability of failure. 

This paper shows a different approach to compute and control optimal trajecto- 
ries for landing vehicles. It provides a new method for computing fuel efficient opti- 
mal trajectories which require minimal thrust modulation. A corresponding tracking 
control scheme is presented which allows the pre-computed optimal trajectory to be 
followed. The robustness of the method is discussed with results of a simulation. 



1 Introduction 

In the future more and more exploration missions will include a landing on the 
surface of a celestial body. Currently missions to the Moon, to Mars and to asteroids 
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are studied. The most critical phase of these missions is the descent and landing on 
the surface of the planet, moon or asteroid. In order to achieve an autonomous, safe 
and precise landing thrust modulation is needed at least for the Moon, Mars and 
large asteroids or moons. Thrust modulation is necessary for three main reasons: 

1 . To allow a fuel optimal descent within a specified safety corridor, 

2. To enable an approach manoeuvre with low descent rates for landing site inspec- 
tion, and 

3. To allow hovering. 

Although not all missions require hovering, a thrust modulation is still needed to 
fulfil the mission. 

For the lunar soft landing missions in the past (Surveyor 1 10|, Apollo Q) throt- 
table engines were used. Since these engines are not available anymore NASA is 
developing |1| a new throttable engine for its Altair lander design H, ||3|. Other 
landing vehicles under study are using two sets of engines. First, a main engine set 
of a few large thrust engines, whose thrust cannot be modulated, is used for decel- 
eration from orbital velocity. Their thrust can not be modulated. Second, the control 
engine set comprises smaller pulsable thrusters which allow the modulation of the 
total thrust. For most of these missions the ratio between total thrust of main engines 
and total thrust of control engines is about 1 to 1 18|. This leads to a large number 
of smaller pulsable engines, thus adding a lot of complexity and risk. 

From this situation the motivation arises to investigate the possibility of trajecto- 
ries and controllers which allow the ratio between main and control engine to shift 
towards the main engines. Thus the number of smaller control engines can be re- 
duced. In the best case the required modulation is created by the reaction control 
system (RCS), eliminating the need for the control thruster set. 

Keeping this theme, this paper will first present the approach for computing opti- 
mized trajectories for landing vehicles which use an RCS and a set of main engines 
in which single engines can be switched off symmetrically during landing. The re- 
sult of the optimization will be a reference trajectory in the altitude-downrange plane 
which minimizes fuel consumption and usage of extra thrust from the RCS for thrust 
modulation. In the next step a controller is developed to follow this trajectory. Using 
simulations, the robustness of this approach is analysed and presented. 

2 Model of Lunar Lander 

In order to calculate optimal trajectories for the powered descent of the lunar lander 
a mathematical model of its dynamical behaviour has to be established. 

2.1 Coordinate Systems 

For the formulation of the equations of motion of the lunar lander, two different 
inertial reference frames are defined. First the equations are derived wrt the xyz- 
frame. Since the initial and final position for the descent will be given with respect 
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to locations on the target planet or moon, the equations of motion are converted to 
downrange d, altitude h and crossrange c coordinates. The definition of the coordi- 
nates is shown in Fig.[T] The transformation assumes a reference sphere of radius r. 
The position can be described in spherical coordinates with the equatorial plane of 
the spherical coordinates equal to the d-h plane. The angle in the plane is denoted 
as 5. The angle perpendicular to the plane is 7. Defining crossrange and downrange 
as the projection on the reference sphere the angles can be expressed as 



r 



c 

7=-- 
r 



(1) 



Definition Pq as the position at zero downrange and zero crossrange, the current 
position P^,,; = [x, y, z) is a function of the angles 5 and 7 and the altitude h 



Po = (0, h + r,Qf , P„, = Rot,(-5)Rot,(7)Po. 



(2) 




Fig. 1 Coordinate definition of altitude h, downrange d. Fig- 2 Definition of landing cor- 
crossrange c. ridor 



2.2 Equations of Motion 



The equations of motions of the lunar lander are deduced from equations with re- 
spect to the inertial Jtyz-frame 



gri'Z 



1 



(3) 



where gjj^. denotes the acceleration due to the gravitational field of the target object 
and Txi-z equals the effective thrust vector. For simplicity the gravitational field of 
the target object is assumed to have a spherical potential 
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go 



(O' - (^' O)^ g.n.z = Rot,(-5)Rot,(r)go. (4) 



The combination of the thrust of the main engines {T,„) and the RCS thrusters (T^, T^, 
Tq) forms the total thrust vector T = (^—T,n — Tu, —7], —Tg^ ■ The effective thrust 

T,3,,-Rot,(/3-5)Rot,(x)T (5) 

depends on the orientation of the lander, which is described by the attitude angles 
/3 (pitch) and x (yaw). Under consideration of the total thrust vector T the latter 
transformation yields 

/rA /cos (jS - ^) iiT,„ + r„)cosx + Tgsinx) - sin(/3 - ^) TA 
Txyz= 72 = sin(/3-^)((r„ + 7;)cosx + 7;sin;f)+cos(/3-^)7:J . (6) 
\tJ V -{T^ + T,)smx + TgCosx / 

Replacing the left hand side of (O with the second derivative of (O wrt time under 
consideration of ( 111415b leads to the equations of motion in the dhc-frame 

/JX / ;M7+feH'=-7 + 72smf)+2d(^tan^-4,) \ 

^'^ V ;M^[(^i^'"7 + 72Cosf)sinf-r3Cosf]-^sm^cosf-||| J 

(7) 

To ensure that the pitch and yaw angles vary continuously, their first derivatives 

are added to the system of differential equations to consider the attitude motion with 
(Oq and ft)^ as commanded angular rates. Finally, 

m = -\T,„\ ■ o,n - (|r„| + |7:,| + |7;,|) ■ Orcs (9) 

describes the thrust-dependent fuel consumption depending on Om and Orcs- For the 
purpose of trajectory optimization and control design, state space representations of 
the dynamics of the lunar lander has to be derived. All states of the dynamical system 
defined by (|7l [H |9ll are gathered in the state vector 

x(/) = (d, h, c, d, h, c, p, X-, m) ■ (10) 

The motion of the lunar lander can be influenced by the translatory and rotatory 
thrusters, which form the control vector u(?) ~ (Tu, T,, Tg, (Op, (0^) . The thrust 
of the main engines T,„ can take only predefined values. The switching times T,-, / = 
1 , . . . , ^, at which engines are disabled, can be collected in a vector of free parameters 
p. With these abbreviations, formulae (|7l[8l|9ll can be written as 
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x(f)=f(x(f),u(0,p,f), x(0)=xo. (11) 

3 Trajectory Optimization 

3.1 Constraints and Landing Corridor 

This mission also requires holding terminal conditions at some terminal time point 



free, 0, mo )^ 
-f, 0, free )^. 

Further constraints for the states and the control inputs are provided, e.g. 



x(0) = ( Jo, 


0, 


0, 0, 


ho, 





x{tf)^i 0, 


hf, 


0, free. 


hf, 


0: 



(12) 



minimum switch on time for a subset of the main engines, 
maximum auxiliary thrust of the RCS thrusters, 
the boundaries of a landing corridor, 
a minimum time for the landing corridor. 



The landing corridor was introduced to ensure that in the final approach phase a de- 
fined amount of time is available for the final slow descent to the landing site. The 
parameters of the corridor are shown in Fig.|2] The safety downrange d^ denotes an 
area around the landing site, where h> hs has to be ensured. The corridor height he 
as well as the corridor angles a^- and ac+ define the admissible domain for the final 
landing path. The time from entering the corridor until reaching the final conditions 
is constrained by a lower bound. For the computation of the optimal reference tra- 
jectory, the motion of the lunar is restricted to a two-dimensional motion in the d-h 
plane. Therefore it is assumed that Tq — T^ — (i)^ = holds. 

3.2 Optimal Control Problem 

There exist many ways to bring the lunar lander from its initial state to its desired 
final position. Optimal control theory provides methods to find exactly that trajec- 
tory which minimizes a given objective functional /, given the system of differential 
equations f, the initial and terminal conditions a and state and control constraints g 

min /(x, u,p) 

x.u,p 

s.t. x(f) ==f(x(?),u(r),p,f) (13) 

a)(x(0),x(Ti),...,x(Ti))-0 

g(x(r),u(f),p,f) < 0, ?e[0,f/]. 

Here, the free initial value for jS (0) is also added to the vector of free parameters 
p. Table [1] gives some possible objective functionals for the optimization of the 
trajectory of the lunar lander. Empirical evidence shows that a linear combination 
of these functionals gives the best results. 
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Table 1 Objective functionals for the trajectory optimization of a lunar lander. 



Fuel consumption 


Auxiliary thrust (with possible 
reference thrust r„.ier) 


Rotation of the lander 


Flight time 


-m{tf)+m{0) 

'' — im — 


'/ 




'/ 
h= I coj dt 


I,=lf 



By switching from a continuous time axis t e [0,f/] to discrete time points t G 
{Q = ti <t2 < ■■■ <ti — tf}, I ^'S^, the optimal control problem can be solved 
numerically. The control function u(?) is also reduced to a discretized version u' ~ 
u(?,), and the discretized state vector x' w x(r,) can be calculated directly from the 
system of differential equations. After replacing the optimal control problem (fT3t 
by the discretized version 

min /(x\...,x',u^...u',p) 



s.t. 



„f+l -^i 



= x' + (/,+i -f,)/(x',u',p,f,) , (• = 1, . . . ,/ - 1 (14) 
co(x^...,x') =0 
g(x',u',p,f,) < 0, ( = 1,...,/, 

which has the form of a standard NLP problem, common SQP solvers can be ap- 
plied. The software library NUDOCCCS |2| follows this method to solve optimal 
control problems. The special structure of this large-scale problem can be exploited 
by sparse solvers, e.g. WORHP |9|. 

The consideration of the corridor as constraints in the optimal control problem 
downgrades the convergence of the iterative solver drastically. To circumvent this 
effect, the optimization is performed in several steps: 

1 . Analysis of the corridor: A descent maneuver is calculated within the corridor 
with free initial velocity to generate reasonable entry values. 

2. Reaching the corridor: The trajectory is optimized until the entry of the corridor 
is reached, where the final state is constrained by the corridor entry values from 
the previous step. 

3. Complete trajectory: By using the previous trajectory as an initial guess and by 
extending it by the corridor phase, a robust solution for the optimal trajectory can 
be found. 

By treating the main descent and the corridor phase seperately in the discretization, 
the constraints for the corridor can be implemented easily: 

• The minimum flight time within the corridor is not allowed to be less than 60 5. 



arctan 



d- 



TffT- 



is constrained by [a^.-,aj.+ ] during the 



• The flight path angle 

corridor phase. 

• The safety downrange is checked by calculating the possibly overlapping area of 
the trajectory and the corridor in Fig.|2l 
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3.3 Results for the Optimal Reference Trajectory 

In order to show the quahty of the results of the optimization the scenario from |l8l 
was selected for comparison. The initial conditions have been chosen in a similar 
way. The difference for the thruster system is that the landing vehicle described in 
13 has four main thrusters of SOON thrust, eight pulsable thrusters of 220 N and 
an RCS with 22 N thrusters. In our example we chose to have a set of eight SOON 
thrusters and an RCS with 22 N thrusters for small modulations of the thrust only. It 
is assumed that the thrusters will be arranged symmetrically such that they can be 
switched off in pairs, e.g. in a sequence of 8, 6 and 4 thrusters running at the same 
time. In addition, the landing corridor is defined with the parameters in Table |2l 
With the given conditions, the optimization of the trajectory provides the solution 
displayed in Fig. [51 As can be seen in the upper left plot, the downrange is about 
SSOkm and the flight path angle is nearly 90° immediately before landing. The 
lower left plot shows the attitude angle /3 of the landing vehicle in the d-h plane 
which also ends with an upright position of the vehicle of 90 ° wrt the horizontal 
plane. The upper right plot shows the corresponding vertical and horizontal veloci- 
ties. In the lower right plot the main thrust T^ shows the two steps at 553 s and 613 s 
where a symmetrical pair of main thrusters is switched off. The second graph shows 
the auxiliary thrust T,, which is needed for the descent through the corridor in order 
to compensate for the decreasing mass of the landing vehicle. The fuel consumption 
for the reference trajectory is 45 .4 % of the initial mass. In order to check the robust- 
ness of the optimization, the initial conditions for the landing trajectory are varied 
over a range, which is similar to the uncertainty for the PDI state determined in |[6l . 
The variation of downrange by ±11 km, altitude by ±700 m, horizontal velocity by 
±0.6 ™/s and vertical velocity by ±0.24 m/s, rendered for all cases an optimal solu- 
tion fulfilling all constraints. The fuel consumption for all simulated cases varied 
from 45.3% to 45.7%. 



Table 2 Parameters of the landing corridor for the reference scenario. 



hs 


ds 


he 


a,.- 


ac+ 


tc 


hf 


3000m 


10000 m 


1000 m 


20° 


100° 


60s 


50 m 



4 Trajectory Control 

The trajectory control of the lunar lander (Fig.|4]) consist of a combination of feed- 
forward and feedback controls, commonly referred to as two degree of freedom 
approach. In order to guide the lunar lander during a landing manoeuvre along a cal- 
culated optimal trajectory Xief(f) the related control signal Uref(?) is used as a feed- 
forward control for the vehicle. To ensure that the lunar lander follows the optimal 
trajectory even in the presence of parameter uncertainties and errors related to vari- 
ations of the initial conditions or external disturbances and despite the fact that the 
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Fig. 3 Result of optimization for reference trajectory 
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Fig. 4 Structure of tlie closed loop in case of the provided 2DOF control approach, whereas 
M denotes the plant and K the feedback controller. 



optimal control Uref (0 has been calculated using the assumptions Tg~Ts — CO^= 0, 
an additional feedback controller is required s.t. the overall control law is described 

hyu{t)=Urefit) + 5u{t). 



4.1 Feedback Control Approach 

For the purpose of the design of the feedback controller the dynamics of the lunar 
lander are represented by the nonlinear state space model as shown in Eq. ( fTTb . 
Assuming that the aforementioned state space representation is analytic and that a 
real landing trajectory is close to the optimal trajectory x(f ) — Xref(?) + 5x(f ), the 
linearization of ( fTTb along x^f (f ) and Uref (0 yields 



5x(f)=A(f)5x(f) + B(f)5u(?) 



(15) 
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dt 



with A(0 = -^ 
ox 



elR"^" and B(0- -, 



Xrer,Uref 



and corresponding initial conditions. The resulting linear time variant system moti- 
vates the usage of a time variant control law that, considering the above-mentioned 
overall control law, leads to 

5u{t) = -K(t)5x(t) => u(0=Uref(f)-K(0(x(0-Xref(f)),f e [fo,f/]- (16) 

The changes in the dynamics of the lunar lander during a landing manoeuvre along 
a reference trajectory are supposed to be slow compared to the sampling rate of 
the discretized version of the optimal control problem jJAl . For this reason the gain 
matrix of the feedback controller K(r) is calculated only at each point f,-,/ G I with 
I = { 1 ,..,/} of the control discretization by minimization of the cost function 



/(5x,5u)=/ 5x{TyQ{ti)5x{T) + 5u{TyR{ti)5u{T)dT (17) 

Jo 

for all i G I. Assuming the stabilizability of the underlying system, Q(f,) > and 
R(?,) > for all ; e I, the feedback matrix that solves the optimal control problem 
is given as a function 

K{ti)=R{ti)-'B{tifSiti), iel (18) 

of the unique stabilizing solution S(?,) of the algebraic Riccati equation 

A{tifS{t,) + S{t,)A{ti)~S{ti)B{ti)R{t,)-'-B{t,fS{ti) + Q{ti)=0, / G I, (19) 

s.t. A(f,) — B(/,)K(f,), ( G I is Hurwitz. To achieve a time-continuous control law, a 
linear interpolation as in the case of the reference control Uref in Section [3]has been 
applied to the K(/,),; G I. 

The main goal for the feedback control is the adherence to the final point regard- 
ing the reference trajectory. For this reason it is desirable to have a time-dependent 
effect on the weighting behavior of the feedback cost function J described by (Til . 
Therefore the weighting matrices Q and R are changed during the control of the 
reference trajectory. At the beginning it is more important to conserve energy than 
to be precisely on the trajectory. However, at the end of the trajectory it is more 
important to reach the final state. For this reason the values of the state weighting 
matrix Q are chosen to be small at the beginning and increase while approaching 
the final state, while the values of the control weighting matrix R are large at the 
beginning and decrease towards the end. 

In case of large perturbations in downrange it is desirable to choose the correct 
point of the reference trajectory with minimum downrange error. In order to do so, a 
time delay is calculated to shift the reference trajectory along the time axis. In case 
of a negative downrange error the lander will fly with zero thrust until the downrange 
is at the desired value. In case of a positive downrange error the lander skips a part 
of the reference trajectory and reference control. 
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4.2 Closed Loop Simulation Results 



The closed loop exhibits the structure shown in Fig. [H where the plant M is given 
by the nonlinear system ( fTTb . To proof the performance of the provided trajectory 
control and to allow a comparison to the results shown in |6J, several simulations of 
the closed loop have been carried out. For this purpose the initial perturbations have 
been varied along grids within the ranges defined in 13.31 The results of different 
simulations of the closed loop depending on the choice of the initial conditions are 
shown in Fig.|5] The contour lines denote the additional mass consumption needed 
by the feedback algorithm to satisfy the final constraints. A landing is classified as 
successful if the condition |zijt(r/')| < 4xmax with 



AXr, 



l^lm/s Im/s im/s iQOm Im 100m 10° 180° free) 



holds, see OH). Note that the provided trajectory control enables successful land- 
ings for each considered initial perturbation. The downrange reference points adap- 
tion described in Section lTT] makes it possible to handle large downrange perturba- 
tions and is almost insensitive to negative values. 

As an example for the detailed results of the closed loop simulations, a single 
specific set of initial perturbations, based on the comparatively high additional fuel 
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Fig. 5 Additional mass consumption for different perturbations as percentage. 
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Table 3 Error in the final point. 



den- 


hen 


CeiT 


den 


hen- 


t'err 


i3eiT 


Xen- 


0.1421m/s 


0.0349 m/s 


-0.0006 ni/s 


-5.3240 m 


-0.0635 m 


0.0070m 


-0.0002° 


-0.0000° 




Fig. 6 Result of the feedback control with initial perturbation of 700m in altitude, 0.24in/s 
in vertical velocity, 1 1000m in downrange and 0.6ni/s in horizontal velocity (solid lines) and 
the corresponding reference trajectories (dotted lines). 

consumption, has been chosen. Applying the feedback law ( IT6l ) to the control sce- 
nario in Section |331 with initial perturbation of 700m in altitude h, 0.24nys in ver- 
tical velocity h, 11km in downrange d and 0.6 m/s in horizontal velocity d, shows 
that the controller is able to reach the final point of the reference trajectory with 
the final error given in Table [3] To reach this result, 0.7% of the initial mass was 
needed additionally as fuel. Fig. |6] shows the controlled trajectory (solid line) and 
the reference trajectory (dotted line) from Section ID 



5 Conclusions 



As demonstrated in the simulations, the proposed methods for optimizing a refer- 
ence trajectory and applying a feedback control law to follow this trajectory are a 
solution to the problem of landing a vehicle on the moon with limited thrust modu- 
lation capability. It was shown that the technique creating a reference trajectory by 
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solving an optimal control problem is robust against variations of the initial con- 
ditions. Furthermore, it was demonstrated that the designed feedback control law 
is able to correct for errors in the initial conditions. For reasons of comparability 
the scenario for the demonstration of the performance of the proposed method has 
been chosen similar to the scenario used in [Sj. The worst-case mass consumption 
for the introduced method is 46.4% of the initial mass of the landing vehicle, re- 
sulting from the worst-case mass consumption regarding to the optimal reference 
trajectories (45.7%) and the worst-case additional mass consumption caused by the 
feedback controller of (0.7%). The simulation results in |8| show a mean value for 
the propellant consumption of 5 1 .37% of the initial mass. From this it follows that 
under the mentioned assumptions lunar landing using the method proposed in this 
paper requires at least 4.97% less propellant mass. Concerning an initial mass of 
1736kg |8 1 a landing maneuver requires 86.2kg less propellant mass than shown in 
IHl. Although these results are very promising a few more steps have to be taken to 
make this approach an attractive alternative to today's concepts. One step is to apply 
the proposed method to one or more planned missions. In order to do so it has to be 
shown that the method also can work for e.g. initial mass changes and thruster inef- 
ficiencies. Furthermore, constraints have to be added for the attitude of the lander, 
especially during the final approach phase, where the landing site must be visible to 
the hazard mapping sensors for a given time. 

To solve the problems for these next steps, some ideas already exist. First, the 
feedback controller should be able to change the time when some of the main 
engines are shut down. Second, more than one reference trajectory can be com- 
puted with different initial conditions and parameters (e.g. initial mass, thruster ef- 
ficiency). Finally the feedback control law can be optimized and designed to guar- 
antee robust stability and performance. 
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Spiraling Approach for Angles-Only Navigation 
within On-Orbit Servicing Missions 



J. Spurmann 



Abstract. On-Orbit Servicing missions possess a navigation problem for the tran- 
sition from absolute to relative navigation when only using camera based relative 
navigation. This gap can be covered by the concept of angles-only navigation. To 
avoid singularities in angles-only navigation specifically trajectory profiles are 
designed. In this context the concept of the spiraling approach is proposed. The 
spiraling approach results from an eccentricity/inclination vector separation estab- 
lished during far formation flight superimposed by an along-track drift to initiate 
far range approach. The benefit for line-of-sight measurements is a change in the 
formation geometry and thus an alternating measurement profile. As a result spe- 
cific maneuvers required during two dimensional approaches to overcome singu- 
larities within angles-only navigation are no longer necessary. In contrast they are 
included in the design of far formation flight and approach strategy. 



1 Introduction 

The capability to rendezvous and dock autonomously to spacecraft in Low Earth 
Orbit (LEO) would provide many options for future space exploration. On the one 
hand the increasing problem of Space Debris could benefit from such technology. 
By autonomously docking to potentially uncooperative satellites at their end of 
life and by de-orbiting them the collision risk within LEO could be reduced. An- 
other field of interest would be the correction of launch errors. By servicing faulty 
injected satellites to correct the orbital elements to the desired ones mission suc- 
cess rates could be increased. In a wider context a fleet management of multi sat- 
ellite constellations could be envisaged. In addition repair of satellites or refueling 
activities close to their end of life could be targeted. In the most severe case the 
servicing satellite could also take over the complete attitude and orbit control of a 
malfunctioning satellite. 
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Due to variety of possible mission scenarios On-Orbit Servicing (OOS) lias be- 
come part of the space programs of tiie US, Japan, Canada and Germany. A mile- 
stone was set witli the successful completion of DARPA's Orbital Express [1] 
mission in 2007. It demonstrated the ability to autonomously perform Rendezvous 
& Docking (RvD) operations including maintenance activities. 

Proceeding one step further some of the aforementioned on-orbit servicing mis- 
sion scenarios desire to capture uncooperative client spacecraft in LEO and to de- 
orbit the coupled configuration. In this context the uncooperative client does not 
provide any attitude or orbit control. Additionally, no docking interfaces or dedi- 
cated reflector pattern for vision based navigation will be available. 

Trying to incorporate those requirements into a navigation concept ground 
based absolute navigation can be performed based on GPS data for the servicing 
spacecraft or radar tracking measurements for the client spacecraft. Those two 
techniques yield similar accuracies of several tens of meters [2]. With the desire to 
create a low cost spacecraft for higher reproducibility the selection of the relative 
navigation sensor is constrained. Low cost leads directly to low mass, low power 
consumption and reduced complexity. Accordingly, radar or LIDAR systems are 
not applicable due to their large mass and power penalties. The only option re- 
maining is thus a camera based relative navigation system. Due to the accuracies 
of the absolute orbit determination the relative navigation has to start at some ki- 
lometers relative range. The operative range of camera based sensors however, 
ends at several hundred meters of relative range [3]. Consequently a gap in the na- 
vigation concept results for the handover from absolute to relative navigation. 

So far this gap has been covered by a method called Angles-Only navigation or 
navigation using Line-Of-Sight (LOS) measurements [4]. This concept considers 
to measure the relative angles between the two spacecraft and to apply those to a 
Kalman-Filter to estimate the spacecraft state. The state can thereby include dif- 
ferent information. The absolute position could be included in different coordinate 
systems, or the relative position vector in a specific frame. Additionally, attitude 
and other orbit parameters as for example the drag coefficient could be included. 
Depending on the relative range however, only a part of the state information can 
be improved by angles-only navigation [4] . 

Upon a closer look on earlier missions utilizing angles-only navigation mainly 
pure along-track separation has been regarded as the driver for the navigation con- 
cept. This is especially represented by the so-called one percent rule used for na- 
vigation sensor selection due to their achievable accuracy [3]. Due to singularities 
in the estimation process of angles-only navigation within those navigation con- 
cepts specific maneuvers have to be performed to alter the flight profile such that a 
variation of the LOS measurements is available and accordingly the estimation of 
relative range with the required accuracy possible [5]. The master thesis of Chari 
[6] performed within the mission analysis of Orbital Express considered out-of- 
plane maneuvers to overcome the singularities for the first time. However, the hy- 
brid trajectories designed by his findings of relative range accuracy within a Kal- 
man filter upon different approach trajectories and applied maneuvers considered 
basically two dimensional approaches with additional out-of-plane maneuvers to 
increase relative range accuracy. 
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The scope of the work at hand will be to apply a three dimensional approach in 
form of a spiraling trajectory to the concept of angles-only navigation. The spiral- 
ing trajectory starts from a safe formation flying ellipse resulting from an eccen- 
tricity/inclination vector separation [7] [8]. Upon application of an along-track drift 
maneuver the servicer starts drifting towards the client in form of a spiraling tra- 
jectory. As a result the maneuvers to overcome angles-only navigation singulari- 
ties are incorporated into the approach trajectory. Further, a safe formation flight 
is guaranteed in the beginning independent of the along-track accuracy since by 
design radial and cross-track component drive the formation flying ellipse. Upon 
approach the ellipse can be shrunken according to the improved accuracy resulting 
from angles-only navigation. 

Accordingly, a trajectory profile to overcome angles-only navigation singulari- 
ties is combined with the formation flying and approach requirements brought up 
by rendezvous and docking of uncooperative spacecraft. 



2 Concept of Along-Track Separation 

Prior to going in to detail on the different concepts it should be mentioned that the 
development of formulas and plots is based on the RTN orbit frame. In this frame 
the R or radial unit vector is aligned with the radial direction (positive outwards), 
while the N or normal unit vector is parallel to the servicer angular momentum 
vector (positive in orbit normal direction). The T or tangential unit vector com- 
pletes the right-handed coordinate system (positive in chief velocity direction). 

To start with the concept description, the gap in navigation accuracy is derived 
within the two dimensional approach concept mainly driven by the along-track 
separation. The design of the navigation concept for a rendezvous or formation 
flying mission is mainly based on the resulting navigation errors. Deriving propa- 
gation errors for circular orbits in the relative orbital frame from the Clohessy- 
Wiltshire equations [3] a radial displacement between the two spacecraft results in 
the largest error in the along-track component after one orbit propagation [3]. In 
case of concentric orbits the error is: 

Arj, = 3;r • Ar^ g 

In case of orbits with a slightly different eccentricity the two spacecraft have the 
same initial velocity, which amounts to an even larger error after propagation of 
one orbit: 

Arj. =12;r- Ar^o 

Additionally, errors from velocity uncertainties are most severe for along-track in- 
accuracies. Those from radial difference are only 20% the size. 

Arj, =3T ■ Avj. 

Thus only regarding a pure along-track separation between the two spacecraft 
and including the effects of velocity errors [3] a rule was derived stating that the 
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navigation sensors must provide a one percent accuracy of tiie relative distance be- 
tween tiie spacecraft for safe approach [3]. 

Considering the formation separated by a pure along-track component at the 
distance of several kilometers the navigation is performed on basis of absolute or- 
bit determination. Ground based orbit determination based on GPS navigation so- 
lution for servicer or radar tracking for client yields accuracies of several tens of 
meters [2]. Hence absolute orbit determination can only be used for phasing and 
far formation at several kilometers relative distance. The vision based navigation 
sensors, which are selected to reduce mass, power consumption and complexity 
are however only applicable up to distances of several hundred meters [3]. Hence 
a gap in the navigation concept arises during rendezvous with uncooperative 
spacecraft within the relative distance of several hundred meters to several kilo- 
meters. A solution to this problem is the concept of Angles-Only navigation. 

3 Angles-Only Navigation 

Angles-only navigation can be implemented to acquire the relative range within 
the mentioned transition zone. The method is well know and widely applied in na- 
val applications, orbit determination, target tracking, lunar and interplanetary opti- 
cal navigation and homing missile applications [4]. 

The relative trajectory between client and servicer can be defined by the rela- 
tive distance r and the line-of-sight (LOS) angles azimuth a and elevation e. The 
basic principle of the angles-only navigation is then to measure the LOS angles 
with the according time very accurately (Fig.l), as soon as the client can be de- 
tected as a moving star in front of the background star field by the far range ca- 
mery (FRC). The corresponding range can be derived from the assumptions made 
above to several kilometers. The obtained measurements are then used to update a 
Kalman Filter, which propagates the orbit onboard the servicer or on ground. 
Apart from an initial guess of the relative range the Kalman Filter uses the final 
spacecraft states from absolute navigation, the orientations of the spacecraft, bi- 
ases (gyro bias or camera misalignment) and noise terms to determine the state 
vector of the client spacecraft [4]. By iterative propagation and a continuous up- 
date process of the filter, the accuracy of the measurements and most important 
that of the relative range will improve. 

The major problem of angles-only navigation is however, the inherent limita- 
tion in determining the relative range with adequate accuracy [5]. If the geometry 
of the relative motion between servicer and client does not change and equiva- 
lently the continuous LOS measurement profile is not altered, a precise determina- 
tion of the relative range is not possible [6]. 

When targeting an approach concept based on along-track separation this prob- 
lem will definitely occur. While resting at hold points or even during hopping ap- 
proaches in along-track direction an adequate navigation would not be possible 
[6]. Thus not only a general change of geometry is important but also the direction 
of applied motion. A solution within this concept is however the application of 
out-of-plane maneuvers [6] . Those force a change of the relative geometry. As a 
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Fig. 2 Angles-Only measurements during fly-around maneuver in RTN frame. 
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result the relative range can be determined with higher accuracy by planning of 
specific trajectory profiles at the handover from absolute to relative navigation. As 
a result from Chari [6] approach of the client including impulsive maneuvers in 
cross-track direction is advantageous similar to approaches in radial direction or 
fly around maneuvers (Fig. 2) within this approach strategy. The limitation of in- 
creasing navigation errors within the relative range while resting at hold points 
remains however. 



4 Concept of E-/I- Vector Separation and Spiraling Approach 

The concept of e-/i- vector separation originally developed for geostationary satel- 
lites and adopted to LEO missions [8] considers slightly different eccentricity and 
inclination of the client and servicer orbit. Accordingly, upon application of paral- 
lel eccentricity and inclination vectors a safe separation in radial and cross-track 
direction is guaranteed to be smaller than 



mm[a- Se,a- Si) 



Thus the servicer is flying in a relative ellipse with the client while being always 
separated safely in radial and cross-track direction. Accordingly, even if the along- 
track component vanishes completely due to its imprecise knowledge the configu- 
ration would still be safely separated in cross-track and radial direction. As those 
two components of the relative position vector can on the one hand be determined 
much more accurately via ground based absolute navigation based on GPS or ra- 
dar tracking data [2] and the error development is at a much smaller scale on the 
other hand a large benefit is given to the collision risk. 
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Fig. 3 Spiral approach trajectory in RTN frame. 
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Upon application of an along-track maneuver the far range approach towards 
the client spacecraft is initiated. The resulting trajectory has the form of a spiral 
and is hence called spiraling approach (Fig. 3). 

Accordingly, a continuously alternating flight profile in all three dimensions is 
available by application of one single maneuver. Hence the problem of singulari- 
ties within angles-only navigation is removed by incorporating one maneuver, 
which has to be conducted anyhow to start the rendezvous approach. To further 
improve the relative range determination accuracy upon approaching the client 
spacecraft in-plane and out-of-plane maneuvers can be applied to shrink the el- 
lipse (Fig. 3). 

5 Benefits from Spiraling Approach to Angles-Only 
Singularities 

In contrast to the required trajectory profile during pure along-track separation the 
e-/i-vector separation provides a huge benefit on angles-only navigation. Consid- 
ering a configuration with a servicer in a relative ellipse with the client spacecraft 
at a certain along-track separation a configuration is found for safe far formation 
flight. 

The benefit of this configuration to angles-only navigation is significant. At 
first the natural motion with in the e-/i-vector separation improves the relative 
range accuracy [6]. Further, the along-track drift alters the flight profile such that 
the relative range accuracy should be guaranteed during approach. In case of re- 
duced filter performance the radial and cross-track maneuvers performed to shrink 
the spiral upon approach improve the performance again. As a result, the singu- 
larities can be resolved. 

A numerical verification of the benefit has to be investigated in further studies. 

6 Conclusion 

It is evident that the concept of the spiraling approach, resulting from e-/i-vector 
separation [7] [8] and an applied along-track drift, provides significant benefit to 
the concept of angles-only navigation [5] [6]. With the continuous relative motion 
between the two spacecraft the line-of-sight measurement profile alternates con- 
tinuously such that a good set of observations is available for setting up the navi- 
gation filter. The inherent limitation of former approach concepts [5] to estimate 
the relative position with adequate accuracy can thus be overruled and the singu- 
larities in angles-only navigation resolved. Especially the required maneuvers to 
change the trajectory profile [5] have not to be designed specifically anymore, as 
they are incorporated in the approach strategy. Additionally a passively safe for- 
mation is guaranteed prior to availability of angles-only measurements. Conclud- 
ing the gap in the navigation concept between absolute and relative navigation is 
elegantly resolved by including the maneuvers required to change the formation 
geometry in the design of the approach concept in form of the spiraling approach. 
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